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ABSTRACT 
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Lagrange formulation of open-loop kinematic chain equations of motion 
for the purpose of Hee laine an generalized dynamic model of robot 
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I. INTRODUCTION 


A. MOTIVATION FOR SIMULATION 

The U.S. Navy is developing robots to help meet the Navy’s 
automation needs. According to VADM Fowler [Ref. 1], ''The 
construction of the 600-ship -(US) Navy requires ever-improving 
productivity through the adoption of the most cost-efficient 
manufacturing technology.’’ A robot, as defined by the Robot 
Institute of America (RIA) [Ref. 2], is: ‘'’a reprogrammable 
multifunctional manipulator designed to move material, parts, tools, 
Or specialized devices, through variable programmed motions for the 
performance of a variety of tasks.’’ Robot manipulator simulation 
allows the determination of arm dynamic responses to various 
permutations of input data by mathematically modelling the proposed 


configurations, 


B. MODELLING AND SIMULATION IN DESIGN 

Simulation software can model diverse components of a robot, even 
within the context of an entire robotic manufacturing cell which is 
comprised of the robot manipulator, end effector, and workspace 
elements. Engineers can use this software to predict dynamic 
performance long before expensive mechanisms have been constructed. 
Physical parameters and design configurations can be selected and 


optimized, and complicated control schemes can be modelled and 
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evaluated. Additionally, engineers can focus their research 
attention on the inherent nonlinear kinematic and dynamic physical 
relationships of a robot manipulator arm to its workcell. 
1. Performance Prediction 
The dynamic response predicted by a manipulator arm simulator 
can be of inestimable value to robotic engineers, designers, and 
users in many areas involving the effects of physical design values 
on performance. For instance, accurate modelling and simulation can 
be used in: 
1. manipulator arm design 
2. robotics educational training 
3. manipulator arm selection 
4. performance evaluation 
5. trajectory planning 
6. trajectory evaluation 
7. workspace layout 
8. control programming 
9. singularity point determination 
10. cycle time determination 
11, dynamic property calculation 
12. actuator/link loads and size determination 
13. error effect evaluation (i.e. backlash) 


14. actuator overload predictions 
15. parameter optimization 


2. Design Parameter Selection 
Manipulator dynamic analysis must aid in the mechanical and 
structural design of prototype arms. Modelling and simulation can 
hasten robot arm development by permitting an engineer to examine 
kinematic and dynamic behavior and better understand the significance 
of various parameters before constructing a manipulator arm. The 
forces and torques predicted for a mechanical arm during movement can 


yield data useful in designing actuators, joints, and structures by 
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making the role of primary design parameters explicit. Discrete 
dynamic coefficients can be numerically evaluated to determine their 
relative magnitude and importance. Additionally, a valid model can 
be used to identify negligible dynamics and the corresponding 
simplified design and programming problem. 
3. Control Schemes 
Robot arm control maintains the dynamic response of a 
computer-based manipulator in accordance with some predetermined 
system performance goal. With a valid model, simulation furnishes a 
testing technique for control strategies without the expense and 
mechanical problems of working with actual manipulators. These 
techniques can also include the off-line programming and testing of 
suitable optimal and sub-optimal control schemes. Control laws can 
be evaluated numerically so that dynamic effects can be resolved 
without inflicting damage-or tying up an actual manipulator system. 
4. Robot Teaching and Workspace Layout 
Robot arm simulation allows development of robot planning and 
programming and can provide significant time savings through robot 


physical modelling and workspace layout. 


C. THE ROBOT MODELLING AND SIMULATION PROBLEM 

The robot arm dynamic modelling problem is one which requires the 
formulation of an accurate and efficient mathematical representation 
of all components involved in manipulator dynamics. Effective 


mathematical formulations are used to predict the amount of actuator 
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torque required to produce a specific motion or they are used to 
predict the motion given the input torque. [In this work the torque 


on the arm joints was assumed as a known function of time, 
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Figure 1, Manipulator Model Block Diagram 


A reasonably complete version of an entire manipulator model 
includes a control system, as shown in Figure 1. Here the actual arm 
dynamics are replaced by a mathematical dynamic model of the 
manipulator. Model accuracy is determined by comparing modelled 
trajectory information (0.8) against actual velocities (,) and 
positions (0). 

This thesis does not consider the mathematical formulation of 
trajectory design, control, or feedback systems, as shown in Figure 
1. It does combine a previously developed mathematical formulation 
with a method of modelling robot arm movements developed by Walker 
and Orin [Ref. 3]. A simplified robot arm simulation problem can be 
stated as: given a manipulator dynamic model, it’s link mass and 
inertia properties, it’s initial link alignments, and the joint 


force/torque signal and limits, then predict the motion of each joint 
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and, ultimately, the end-effector (i.e. individual displacements, 
velocities, and accelerations versus time). The simulation is not 
updated with actual velocity and position data, as is the model of 
Figure 1, but runs on its own predictions, as shown in Figure 2. 

Simulation accuracy is evaluated by comparing predicted values (@.) 


to actual values (@). 










Dynamic 
Model 





Integrator 


Figure 2. Manipulator Simulation Block Diagram 


1. Current Solution Methods 

Many methods of accomplishing computer simulation of 
complicated manipulator mechanisms have been developed recently. An 
examination of the more prominent simulation techniques was made and 
a summary of these follows. 

Vereshchagin developed a method which uses a direct 
implementation of Gauss’s principle of least constraint [Ref. 4]. 
Nelson and Chang explored nonlinear motor current and voltage limit 
effects when simulating a specific robot arm having three Cartesian 


axes of motion with two rotational wrist axes [Ref. 5]. 
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Murray/Neuman and Cesareo/Nicolo have developed a computer 
program which can symbolically generate the dynamic equations of 
motion [Ref. 6,7]. 

Meyer/Jayaraman, Leu/Mahajan, Wang/Lin, and Staufer showed 
the practical use of graphics in simulation software [Ref. 
8,9,10,11]. Backhouse described a dynamic simulation which was 
implemented on an analog computer and included the modelling of 
hydraulic motors and servo-valves [Ref. 12]. 

Brandeberry/Dufour/Spalding have formulated specific dynamic 
equations for a small robotic arm they are designing [Ref. 13]. 
Cvetkovic/Vukobratovic developed dynamic modelling using the Newton- 
Euler formulation with recurrence relations for velocities, 
accelerations, and forces [Ref. 14]. Derby has considered simulation 
using cam-motion profile techniques with the General Robot Arm 


Simulation Program (GRASP) [Ref. 15]. 
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II. BASIC MANIPULATOR CONCEPTS 


A. MANIPULATOR ARM CONFIGURATION 

An understanding of manipulator kinematics, statics, and dynamics 
is fundamental to designing a simulation system. The Robot 
manipulators considered in this thesis are composed of serially 
connected, rigid linked structures, jointed by one-degree of freedom 
joints. Manipulator joints which rotate about an axis are called 
revolute and those which change their position along an axis are 
called prismatic. The principal degree of freedom for revoluate 
joints is the joint angle, and for prismatic joints is the translated 
distance. At the end of a manipulator chain, there is usually some 
tool or hand/gripper mechanism which performs the actual task for 
which the robot was designed. Due to their variety and complexity, 
the devices placed in this position are designated as end-effectors. 
Joint actuators are typically located between adjacent links to allow 


proper joint positioning. 


B. KINEMATICS 

The basic robotics problem is to perform a task. Motion planning 
is needed to map the composite motions required for each overall 
task, In planning manipulator movement, the primary task is to 
achieve a specified end-effector Cartesian position with respect to 
the robot’s environment. A kinematic evaluation is used to represent 


the positional relationships of the manipulator mechanism. 
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1. The Direct Kinematics Problem 


In direct kinematics, a joint coordinate system is translated 
into a fixed coordinate system. Fixed or ‘'’world’’ coordinate 
systems must be established and the origin can be chosen at any 
location, including the foundation of a conveyor belt, a central 
geographical location, or even the base of the robot, For the 
purposes of this thesis, the robot base will be used as the fixed 
coordinate system origin, Further details concerning this are 
provided in section II.A.3.b which follows. The direct kinematics 
problem is: given the robot arm’s joint angle vector, find the 
manipulator’s end effector position and orientation with respect to 
the fixed coordinate system, This problem has an analytically unique 
solution, [Ref. 16] 

2. The Inverse Kinematics Problem 

The inverse kinematics problem can be stated as: given the 
position and orientation of the end effector with respect to the 
fixed coordinate system, find the robot arm joint angles. This 
problem does not have a unique solution due to the transcendental 
nature of the kinematic equations. [Ref. 16,17,18] 

3. Link Representations 

A link coordinate frame (attached to the body) is shown in 
Figure 3. A summary of the link numbering convention and specific 
procedural steps for establishing link axes and defining link 


parameters are provided in Appendix A. 
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Figure 3. Link Coordinate Convention 


An essential part of the kinematics problem is to find a 
transformation matrix that relates the body-attached link coordinate 
frame to the fixed coordinate frame. General solution methods for 
the kinematic problem have been proposed by several researchers [Ref. 
18,19,20,21] and summarized below. 

a. Denavit-Hartenberg Convention. 

Denavit and Hartenberg were the first to relate spatial 
configurations between neighboring robot arm links using a matrix 
representation of rigid link kinematics [Ref. 22,23]. Vicker 
extended these methods to a generalized displacement analysis [Ref. 


22,24]. Under Denavit and Hartenberg’s well developed and popular 
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convention, the kinematics solution for each link’s position and 
Orientation is founded upon a 4x4 homogenous link transformation 
matrix, The individual Denavit-Hartenberg link coordinate matrix, or 
A matrix, is a 4x4 transformation matrix from link i's coordinate 
system to link (i-1)'s coordinate system as shown in Figure 3. The 
4 matrix describes the first link’s position and orientation 
' relative to the robot base. by describes the second link’s position 


and orientation relative to the first link, etc. The A matrix is 


expressed as [Ref. 22]: 


cos @, | -sin 6. cosa, | sin ®. sina. | a. cos 0. 
i = i : i i at i 
sin @. | cos 6. cos a | -cos ©. sina. | a. sin 9. (1) 
he i, i —— i 
0 ; Sin a | cos a, | d, 
i + 
0 | 0 | 0 | 1 


and A, = I, the identity matrix. 

The coordinate system for the manipulator used in this 

thesis, the PUMA arm, is shown in Figure 4. 
b. Transformation Matrices. 

Once an orientation for each link is established by it’s 
link transformation matrix, than any link position can be established 
in fixed coordinates. For example, transformation matrices are used 
to map the link 6 (end-effector) position and orientation vectors 


into the robots base coordinate system, as shown in Figure 5. In 


24 





PUMA Arm Link Coordinate System 


Figure 4, 


25 


general, the transformation matrix, T. is obtained by chain 


multiplying successive link coordinate transformation matrices, A., 


or: . i 
T = ITUA. ; for i=1,2...n (2) 
oO Piet j 
j=1 
6 _ - 
For example: ie = Te = AA, A, A, A, A; 





Figure 5. Position and Orientation Vector of End-effector 
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The transformation matrix is comprised of four submatrices as follows 


[Ref. 19,20]: 














yi | noa | Pp | Orientation Vectors | Position Vector 
o=|— ae |= ea =. aa -|-----— See (3) 
ooo | 1 Perspective Transform | Scaling Factor 
n a n oO a 
te = [ . . ss o | = n= oO az 5 (4) 
n o ay p> 
07.0" Om 1° 
where: 


n = end-effector normal vector. (Orthogonal to robot arm fingers, 
with a parallel jaw hand.) 


o = end-effector orientation vector. Points in finger motion 
direction. 


a = end-effector approach vector. Points in direction normal to 
hand base. 


p = end-effector position vector. Points from robot base 
coordinate system to origin of hand coordinate system, 


c. Position vectors. 
2 T 
A position vector r can be represented as ices tot at) : 
The ‘1’ allows rotation and translation combinations when applying 


transformations. This is done by multiplying a position vector by a 


link coordinate matrix, as follows: 


Let *r = vector from link i's origin to a point, 
expressed in the link i coordinate system. 
a1 : ee : 
r = vector from link (i-1)'s origin to the above point, 


expressed in the link (i-1) coordinate system. 
Therefore: 


ee. (5) 
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Similarly, a vector can be defined from the i 
coordinate system to a fixed point in link j, but expressed in link i 
coordinates as “r. = (tot sz 1)". Therefore, points in adjacent 


J 


coordinate systems are related by: 


aatA ee (6) 
J 1 J 
Thus, any positions in two different coordinate systems i and j can 


be related by cascading the transformations: 
*ro = T Jer (7) 
J 
with a as defined in Equation (2). The matrix Tr = I. The 


superscript, 0, is omitted when referring to the base coordinate 


k k 


= j 
ry T, r (8) 


system, thus it can be written r, = or and ae = T,. Therefore, 


d. Euler Rotation Transformation Conventions, 
In addition to the above, robot arm link orientations can 
also be expressed by a sequence of rotations about the x, y, z axes, 


These rotations can be in the form of Euler angles: 


Euler(%,0,%) = Rotate(z,8), Rotate(y,0), Rotate(z,#) (9) 


¥ about the z axis 
© about the new y axis 
® about the new z axis 


The Euler homogeneous tranformation matrix would then be: 











(10) 





R(#,0,%) | p | Rotation | Position Vector 


o o o | 1 Perspective | Scaling Factor 
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C. TRAJECTORIES 

Trajectories consist of the time function of the positions, 
velocities, and accelerations of points on the arm. As previously 
stated, to move a robot manipulator end-effector to a point in 
Cartesian space the individual link joint angles must be found, The 
task of moving the end-effector from one place to another in a timed 
sequence, requires the formulation of a trajectory plan. This can be 
done by converting desired end-effector movements into time- 
serialized movements for all of the manipulator joints. Once 
formulated, each robot arm joint will move through the sequence 
trajectory points. 

The specific values for position, velocity, and orientation 
depend upon the type of trajectory to be followed between workpoints. 
Various methods have been developed to determine trajectory plans 
[Ref. 19,25,26,27]. For example, a common class of trajectories 
corresponds to the straight line movement of the end-effector between 
workpoints., Additionally, trajectory plans can depend upon whether 
continuous (smooth) or point-to-point motion is desired. Trajectory 
planning must not produce trajectories which exceed actuator bounds 
Or arm stress limits. However, the trajectory planning process is 
seen to be beyond the scope of this thesis. Trajectory values used 
to verify simulation and modelling results were obtained from 


previously published data for the specified manipulator arn, 
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To move a manipulator along a trajectory in accordance with the 
prescribed plan, torques must be exerted by joint actuators. The 
forces/torques required must be computed. The relationship between 
kinematic states and driving torques is seen during kinetics 


computations, 


D. KINETICS 
System dynamics, which determine the effects of torque on the 
manipulator, are complex due to multiple degrees-of-freedom and 
complicated interlink relationships. Of principal concern in this 
thesis are those internal forces and moments generated as a result of 
torque application and subsequent motion, Forces generated by 
payload and obstacles are not modelled, 
1. Statics 
Static forces exerted by the manipulator on the environment 
and static link forces should be evaluated in robot modelling. An 
equilibrium analysis of the manipulator system at rest is performed 
by equating all external forces acting on the robot to zero and 
computing the corresponding internal forces, A static force 
evaluation was not done for this thesis. 
2. D’Alembert’s Principle 
Significant contributions to theoretical dynamics were made 
during the eighteenth century by J.R. d’Alembert (1717-83), L. Euler 
(1701-83), and J.L. Lagrange (1736-1813). D’Alembert’s principle 


describes a viewpoint where a body’s acceleration generates an 
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inertia force which is considered with the other forces acting on the 
body [Ref. 28]. This concept of dynamics, with an inertia force 


’ equilibrium. 


formulation, creates an artificial state of '’dynamic’ 
The ae ternad forces acting on the manipulator are summed and equated 
to '’d’Alembert’’ forces. 
3. Dynamic Equations of Motion 

Manipulator dynamic equations relate forces/torques to joint 
angle positions, velocities, and accelerations by taking into account 
link masses and geometic properties. These equations can be 
formulated and solved using one of several approaches. For example, 
one or two link mechanisms can be formulated directly from proper 
free~body diagrams [Ref. 31]. With simple mechanisms, the entire 
free-body system can be described in a framework of ‘’’inertial 
coordinates’’, However, for more complex systems, Lagrangian, 
virtual work [Ref. 32], or Newton-Euler [Ref. 19,21,33] formulations 
must be used, 

4. The Dynamic Simulation Problem 

The dynamic robot arm modelling and simulation problem is 
restated as follows: Given a manipulator dynamic model, it’s link 
mass and inertia properties, it’s initial configuration, and joint 
torque signals; predict the joint positions, velocities, and 
accelerations which are be produced at each joint and the end- 
effector. (In the inverse manipulator dynamics problem, which forms 


the heart of control programming efforts, joint torques. are 
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determined from the relationships of joint positions, velocities, and 
accelerations.) 
a. Dynamic Equation Simplification. 

Torque signals must be quickly and accurately computed to 
provide a manipulator with proper control power to carry loads along 
planned paths. In order to accomplish this real-time control, they 
must be computed on the order of at least 60 times per second [Ref. 
27Ae Therefore, there has been strong pressure to increase 
computational efficiency, allowing dynamic equation solution in real- 
time control systems. [Ref. 34,35]. Additionally, as mechanical 
design and control engineers use dynamic analysis to develop better 
manipulators, there will be an increased need to further decrease 
computer simulation time and costs. 

Many analytical schemes have been proposed to make the 
real-time dynamics predictions computationally “feasible. Most 
Significant are dynamic simplifications which ignore some terms and 
correct errors with feedback [Ref. 21]. A good understanding of 
terms and their significance must _ be developed to allow 
simplification of the dynamic equations of motion in controllers. 

The most common method of simplifying dynamics has been 
to ignore Coriolis and centrifugal forces, which represent the 
greatest computational burden in dynamics calculation [Ref. 21]. 
Other assumptions include the consideration of some system elements 
as massless and the neglect of certain joint offsets [Ref. 193 page 


31]. 
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Results obtained in simplification are typically valid 
only in specific ranges of operation. Coriolis and centrifugal 
forces cannot be ignored at high movement speeds. For example, it is 
not a good approximation to neglect the velocity terms except at the 
beginning and end of the arm motion [Ref. 33,36]. 

Another simplification includes the ignoring of actuator 
dynamics. When actuator dynamics are considered in manipulator 
dynamics, higher-order differential systems result, This high order 
system is not readily solvable [Ref. 293 page 71]. Therefore, 
dynamic analysis is greatly simplified if actuator and rigid 
manipulator link dynamics are separated in the simulation approach, 
The effects of actuator dynamics is considered beyond the scope of 
this thesis. 

Computational reductions using model reformulations have 
also been presented which use various matrix, vector and numeric 
analysis techniques. These methods are discussed in the sections 


Which follow. 


E. THE LAGRANGE FORMULATION 

The first manipulator dynamics formulations were based on 
Lagrange equation derivations [Ref. 29] and were so inefficient that 
torques for specific trajectories could not be computed in anything 
close to real-time requirements, Although this is a serious problem 
in manipulator control, Lagrange equations were one of the principal 


methods of simulation and analysis for almost a decade [Ref. 29]. 
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Through persistent efforts to increase computational efficiency, 
Lagrange equations now provide the foundation for current, more 
effective real-time control methods. Discussions of Lagrangian 
equations can be found in most dynamics textbooks [i.e. Ref. 37]. 
The Lagrange formulation can represent the dynamic behavior of rigid 
body systems. Kahn was the first to apply a general formulation of 
the Lagrange equation specifically to robot manipulators [Ref. 38]. 
Uicker adapted Kahn’s work for open chain manipulator linkages, using 
homogeneous coordinates and the transformation matrices discussed 
earlier [Ref. 24]. 

The chief advantage of Lagrange formulated manipulator equations 
is in the straightforward and simple methodology, since the six 
manipulator links are each consecutively referenced to the preceeding 
link, Lagrangian generalized coordinates thus lead to a systematic 
representation of multilink mechanisms. 

1, Energy Calculations 

The Lagrange manipulator model formulation begins with the 
formulation of kinetic energy (KE) and potential energy (PE) 
expressions [Ref. 24]. 

a. Kinetic Energy. 

The critical difference between the Lagrangian-Euler and 
Newton-Euler methods comes in the method used to formulate kinetic 
energy. As previously stated in equation (8), a position vector to a 


point (for example a differential mass) in base coordinates, with 
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respect to link j when i=0 (for the base) can be written <> T, ry 
or more simply, fr > ie Jy, Velocity can then be written: 
j 
5 eee eel eis 
dt ar (cp) 
k=1 ° 4s 
and 
v2 = [ dr ir _ ; a T 
Fa r°r tr (rr) (12) 
j j OT. oT. JT 
v* =tr ) ) : r en a - 4.4 (13) 
9a, Jj € 3) dq k“p 
k=1 p=1 
where: 


qd; = joint variables, the generalized coordinates signifies the 
joint angle, 9., for single degree-of-freedom rotary joints 
and displacement of sliding joints. (Multiple degree-of- 
freedom joints are modeled as single degree-of-freedom joints 
with intermediate links of zero length and mass.) 


Since kinetic energy = 1/, mv*, then the kinetic energy 


GRE of a differential mass at point i, rs: 
i Spe ety 
dkE > tr (“ r( rs) ) dm (14) 
eI oT. dT. JT 
= ) ) tr mee (Jy Jt dm) mal f3 
2 aq, dap | kp (15) 
k=1 p=1 


In the above, a trace operator forms the tensor product Jy aren from 


which the inertia matrix =e is found, 
The total kinetic energy ie of a link is found by 


integrating over all the differental masses in all the links and 
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summing each link’s kinetic energy — with respect to the inertial 


frame: 
<0 eed : Seeded F 
KE, => | ake, 3 tr CT, Se Je") am T, ) (a 
1 e e 
where: ; iT 
J, = f(Jr Jr") dm = the inertia matrix (18) 
J. = inertia tensor with respect to the joint situated next 
J to link j expressed in j's coordinates 
o ‘i £ 
+ = 
: : 2 2 es 
Lae es = 
2 Jxzy Jj XZ J 
2 2 2 
3 SE. Pk. 3 es 
+ jxx jyy jzz . 
J.= o, 12% 2 ss J} (19) 
J J 
2 2 2 
+ - 
2 z ieee K ivy a = 
j xz jyz 2 j 
x, y Zz. 1 
with: J J J 
re = radius of gyration of link j about the j-kK axes 
B = link j mass 
a = transformation matrices representing both linear and 


angular link velocities. 
Therefore, the manipulator structure’s kinetic energy is: 
aT aT qT 


i 2 
: zs i ) ) See 
oe ) f on ) grees aq, J aq k (20) 
j =] j =] =1 p=1 p E 


Qe 
Qe 
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The actuator kinetic energy at the joints is: 


Nactuator, - (21) 


Summarizing, the total kinetic energy is: 


1 1 
@ TT | P ; 1 
> > [ Cae el, 4s 8 


b. Potential Energy. 


n 
<4 
> Ta, 4 (22) 


The potential energy of a individual link can be 


expressed as [Ref. 19,21]: 


PE = mgh = —mg.rer (23) 
where bs 
r = position of the link’s center of mass 
g = the gravity vector lel = 9.8062 m/s? 
m = mass 
h = height 


For a link with a mass center at a position vector ee 


with respect to link j coordinate frame T; is: 


PE = -m. g' T, Jr. (24) 
where: J J J 
Jy. = a vector from link j origin to its center of gravity 
m = the link j mass 


The total robot arm potential energy is the sum of each 


link’s potential energy Se expressed in the base coordinate frame: 


n 
PE = ) a aPE, 
j=l j 


ay T is 
Baa i (25) 


il 
lt M1 6B 
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2. Lagrange Equation Formulation 
After the Lagrangian, L = KE —- PE, is established it is 
applied to the Lagrange manipulator formulation to produce the 
generalized force required for joint i to drive the ith robot arm 
link, Symbolically, we can define the forces as: 


d dL dL 
f -_— es = a £= 1, .6.cvs0 (26) 


i 
dt oq; oq, 


The dynamic equations of motion for a n link manipulator as 


derived for the formulation of the Lagrangian function are [Ref. 30]: 


i aT, oT. 


n j j at a°T, 
f. =) ) tr 7 dy ) ) tr ae 4,4, 
i oq; a, oq, 64,94 
j=il k=1 k=1 p=1 
T oT. . 
a m8 za "t, 1 = Tee 6 5 0 (27) 


This dynamics equation is a closed form expression, and shows 
that an applied joint torque is explicitly dependent on all joint 
movements, The equation’s complexity comes from the dynamic 
interactions between each of the manipulator links, 

For purposes of the following discussion, it is important to 
recognize that there are three types of terms in the Lagrangian 


dynamic equation. They are: 


(1) inertial forces which are proportional to the joint 


accelerations 4, 
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(2) velocity forces which are proportional to the product 2 
a. centripetal forces when k = p and proportional to q?, 
b. Coriolis forces when k # p. 

(3) gravity forces 

3. Lagrange Matrix Formulation 
The manipulator arm dynamics of equation (27) can be 


rewritten in matrix-vector form [Ref. 6,39]: 


n 
) “or eae 3 (28) 
or, more compactly (adding the effects of outside forces): 


F(t) = H(q)4° + C(a,a)q + G(q) + K(@) kK (29) 
In the Lagrangian formulation, the dynamic model parameters for the 


above are [Ref. 3,6,20]: 


H(q) = Inertial coefficient (nm x n) matrix 
C(q,q) = Centrifugal and Coriolis force m-vector 
G(q) = Gravitational force nm-vector 


K(q) Jacobian matrix of joint forces created due to 


external forces exerted on link n 


k = n vector of external force exerted on link n 


4. Limitations in the Lagrange Formulation 


The dynamics computation using the Uicker Lagrangian 
formulation, equation (27), is far too slow for use in real-time 


control or in simulation. Luh, Walker, and Paul found that 7.9 
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seconds was required for each evaluation of the Uicker Lagrangian 
[Ref. 27] for a six link manipulator. As can be seen from equation 
(27), a triple summation is evaluated for each joint torque to obtain 
the velocity product terms. Although it can be argued that the 
velocity products can be neglected [Ref. 19,21], another 
computational inefficiency exists in the double summations that must 
be computed at each joint for the acceleration terms. The double 
sums are: (1) the inside bracket summation, ranging from joints 1 to 
j, and (2) the outside bracket summation, ranging from joints i to n. 
This results in computational operations proportional to 7 when a 
triple and double summation is considered with joint torque 
computation for n joints. Therefore, reformulation is necessary to 


satisfy real time control requirements. 


F, RECURSIVE LAGRANGIAN DYNAMICS 

In 1981, Hollerbach substantially reduced the Lagrangian method's 
computational requirements by establishing a simple, succinct 
formulation of (forward) link numbering recurrence relationships 
[Ref. 40]. Hollerbach’s reformulations decreased the number of 
computational operations to the order of n. Additionally, Waters 
found generalized forces could be expressed in a manner that that 
took advantage of backward link numbering recurrence relations [Ref. 
Zoe 

The structure of forward and backward recursions contributes 


greatly to increased computational efficiency. Each approach yields 
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equivalent equations, through (1) a backward recursive computation of 
the linear and angular velocities and accelerations from the base to 
the manipulator end, and (2) a forward recursive computation of 
generalized working forces and torques from end to base. Recursive 
dynamic equations can compute angular velocity, linear and angular 
acceleration, and forces and torques, and achieve great efficiency 
because they only execute recursions once, thus avoiding costly 
matrix summations. 

Hollerbach’s reformulation also showed that the number of 
coefficients can be cut in half by using 3x3 transformation matrices 
and explicit position translations insteads of the 4x4 homogeneous 
transformations discussed earlier, There are many inefficiencies in 
4x4 matrices due to the many zeros, ones, and redundancies in 
original A; matrices. For purposes of this thesis, however, 4x4 
matrices are used, Greater computational efficiency has been 
sacrificed here for greater flexibility, and compatibility with many 
published research articles. 

As stated in section II.5.b, the Lagrange formulated equation for 


obtaining generalized forces for an m-link manipulator is: 


E j iad ae jo a 27 
f{, = ) ) tr vs dy + ) ) tr} . ar saree: 4.4, 
= “ = 


i aq. 7 dq dq. ) dq,0q 
jail k=1 7 =1 p=1 2 oy 
oT. 
= ee — ce 
J aq (27) 
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where: 





ey = Ty-1 _ ; k < j (30) 
dy dy 
07T. Qo2 
— t= T_ “kk AR (31) 
aq,9q te aq2, 4 
kK ty k 


Hollerbach showed that to obtain generalized forces, the 
computational procedure requires: 

(1) computation and storage of the = terms 

(2) computation and storage the dT. /dq, and 8°T/8q,94, terms 


j 
1. Lagrangian Dynamics with Backward Recursion 


The derivation of the generalized forces is: 


n 
oT. sue OT. 
fers ) tr i r -m cee Jy 
i aq j j j aq j (32) 
j=i i i 
where; 
T, =T A (33 
j j-i1 3 
; : oA, 
T. = T. A. + T. — q. 34 
j jr1 J j-1 , 45 i 
q 
j 
a = : oA, O2A. OA. 
- A +27, —2 6.77, — G2 7a (35) 
j j-i jj jel gg, 5 j-1 5,3 jr1 4, J 
3 3 a 


The initial conditions for the base, the linear and angular 


e 
velocities and accelerations T, and T)» are zero, since the 


manipulator base does not move, 
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2. Forward Recursive Lagrangian Dynamics 


The next step in the recursion formulation clarifies how 
forces/torques are reflected back from manipulator tip to base. This 


forward recursion leads to increased Lagrangian dynamics efficiency. 


i oT oT, 
dq, oq. uy 


The genralized force equation (6) becomes: 


n aan n ee 
» ot ee ices i Sm te 
mea tr | — jj J ——= jj J 
7 Q 3 (36) 
ae ana 
where: 
nN 
eee ) T, J; a ner v + AS iad (37) 
ja 
nN 
c = ) m Tr Jy = m he + A Cc 
i i er itt “itt (38) 
jai 


Substituting the D. and C. terms into equation (36), the forces are: 


oT. T oT. 
f. = tr — D. =~ % — Cc. 
oq, oq, (39) 


This recursive formulation is computed by: 


(1) computing T, terms from i=1 to i=n using equation (39) 


(2) computing D. and c, terms from i=n to i=1. 
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The second step is where computational complexity of an n 
link arm is reduced from the order of a to n, The only way to 
further reduce this linear complexity would be to reduce the number 
of linear polynomial coefficients, which will not be done for the 
generalized program used in this thesis. 

Although a recursive Newton-Euler formulation is almost three 
times more efficient that a recursive Lagrangian formulation [Ref. 
40], there appears to be no fundamental difference between the two 
formulations when properly configured kinematically [Ref. 41]. 
Although not as efficient, the Lagrangian formulation has been 
selected here because it offers straightforward programming 
algorithms which do not depend on specific manipulator 


configurations. 
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III. MOTION MODELLING AND SIMULATION 


A. MODELLING AND SIMULATION APPROACH 

The modelling approach in this thesis is a Lagrangian description 
of a manipulator arm’s dynamic behavior, The model computer program 
utilizes a general-purpose kinematic analysis algorithm which 
includes both forward and backward recursive Lagrangian solutions. 
These algorithms are based on coordinate frame representations for 
the links and formal Lagrangian mechanics interpretations. The use 
of generalized algorithms allows future evaluation of many different 
dynamic models within one common software program using a data base 
of various arm parameters. Additionally, a generalized program can 
be simplified for specific manipulator configurations. 

The individual control torques used in this program are provided 
in the form of input data. A given control force is used to compare 
the simulated dynamic response against a known experimental response. 

For each time step, two tasks are involved in the simulation: (1) 
the robot arm trajectory is determined through integration of 
acceleration and velocity for assumption of constant acceleration, 
and (2) the joint accelerations for the next time step are obtained 


through solution of the dynamic equations of motion. 
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B. COMPUTATION CONSIDERATIONS 
1. Simplifying Assumptions 
Because of equation complexity, past researchers have used 
simplifying assumptions for the mechanical model so that a solution 
could be obtained [Ref. 293; page 62]. This program includes all 
terms which appear in the Lagrangian formulations as shown in 
equations (27) and (32). 
2. Computational Efficiency 
Current research emphasizes computational efficiency to allow 
a real-time dynamic equations solution for control schemes [Ref. 27]. 
Additionally, increased computational efficiency promotes simulation 
cost optimization. The emphasis of this program is to provide the 
foundation for future control work and to offer as general a model as 
possible. Therefore, the program was not designed as the most 
computationally efficient, although efficient software schemes were 
used where possible. Hollerbach has concluded that 4,388 
multiplications and 3586 additions are required for the formulation 
selected for use in this program, Reductions to 2,195 
multiplications and 1,719 additions could be made using 3x3 
transformation matrices [Ref. 40]. 
3. Structure Flexibility 
All manipulators are flexible to some extent. However, the 
rigid-body dynamics presented here are an idealization for a real 


manipulator. [Ref. 43] 
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4. Integration 


Currently, the standard procedure to solve a nonlinear system 
is to use an iterative numerical time integration method on the set 
of nonlinear differential equations describing the system [Ref. 44]. 
The thesis program as currently designed will analyze a rigid link 
manipulator using a trapezoidal numerical time integration algorithm 
[Ref. 45]. Through the assumption of constant joint acceleration 
during time steps, no iteration is necessary, 

9. Frictional Forces 

Frictionless conditions were assumed for the thesis 

simulation, 
6. Vibration and Dynamic Stabilty 
Vibrational and system dynamic stability effect were not 


considered in this thesis work. 


C. PROGRAM APPROACH 

The FORTRAN 77 language was chosen to implement the dynamic 
formulation, The source code produced for this program was complied 
on an IBM 3033 computer using the VS FORTRAN compiler (Release 3.0). 
The methodology and programming philosophy used in generating the 
source code are provided in the sections which follow. 

1, Principal Program Matrices 

In robot arm modeling, five terms (inertia, Coriolis, 


centrifugal, gravity, and external) make up the joint forces or 
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moments, Equation (29) can be rewritten to model the dynamics of a 


revolute six-element open chain linkage in the following manner: 


< = H(6)@ + C(@,6)6 + G(@) + K(6) 7k (40) 
where: 
H( 9) mn X mn symmetic, nonsingular moment of inertia matrix 
c(@, 6) mn xn matrix of centrifugal and Coriolis effects 
G( 6) n x 1 vector specifying the effects due to gravity 
K( 0) 6 x n Jacobian matrix of torques created at each 


joint due to external forces exerted on link n 


K(e)! transpose of K(@) 


k 6 x 1 vector of external forces exerted on link n 
t n x 1 vector of torques of each joint actuator, 
9 n x1 vector of joint variables 


Note in the above that the joint torques are linear functions of the 
joint accelerations, 
2. Modelling Solution Methodology 

The methodology used to validate the dynamic model is shown 
graphically in Figure 6. Here, the torque was given for a specific 
instant in time. This was then combined with the present 
experimental angular position and velocity as input for equation 
(40). External forces other than gravity where not considered with 
the input data available. The angular acceleration for the same time 
instant was then computed from equation (40), A simple integration 
was then performed over the acceleration time domain to provide a 


velocity and position check with actual data. Figure 7 provides a 
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<(t) = H(e) © + c(e,e) + 4G(e) 





Figure 6. Model Formulation Diagram 
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Figure 7. Computer Program Flow Diagram 
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block diagram of the steps (and associated subroutines) which were 
used to implement this methodology. 
3. Simulation Solution Methodology 

The methodology used to simulate the dynamic motion is shown 
graphically in Figure 8. As before, the stepped torque was provided, 
with only the initial velocity and position. The time interval 
between knots was divided into equally sized steps of length At. The 
acceleration at step i was assumed equal to that at itl and a 
standard forward finite difference formulation was used to determine 


the estimated velocity and position, 9. and @ 


i+] i+1? at time ti. 


fe 
These values were again applied to equation (40) to recompute the 


acceleration at time t for the next time step. This acceleration 


i+1’ 
was then reapplied to the finite difference formulas to determine the 
next positions and velocity, etc. The step size was varied to 
determine the effects on computer run times and accuracy. 
4. Principal Subroutines 

The following descriptions apply to subroutines found in a 
subroutine called ROBARM. ROBARM was called by a main program as 
often as necessary to evaluate the particular dynamics at a specific 
knot. The recursive Lagrangian representation of the dynamic motion 
equations [Section II.F] was used in the simulation method developed 
by Walker and Orin [Ref. 3] to create a FORTRAN subroutine named 


ARM1., ARM1 allows input of position 0, velocity 0, acceleration 9, 


external forces/moments, and joint forces and torques. 


31 


Given 


Given 


+ 6,At => @O 


‘A 
om 
© 





Given 


+ G(6) 


<(t) = H(6) © + C(6,6) 





At 


Simulation Formulation Diagram 


Figure 8. 
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The FORTRAN subroutine, ARM1(0,6,0,k,), ultimately computed 
H, the inertia term matrix, given 6,0,0 and the forces and moments 
exerted on link n, EK. Another FORTRAN subroutine, ARM2(0, 8,1), is 
identical to ARM1 (0,0, 0, k, T) except that programming code for 
velocity terms, gravitational effects and external forces/moments 
effects, has been eliminated. Subroutines ARM1 and ARM2 form the 
basic programs used for simulating the rigid body manipulator 
dynamics. An explicit analytic expression for each of the terms of 
motion equations is not required for the simulation, 

5. Determination of Acceleration 
A bias vector, b, is set equal to the torques due to gravity, 


centrifugal and Coriolis accelerations, and external forces and 


moments on link n. Therefore, 


b = C(6,6)6 + G(@) + K(@)- & (41) 
Thus, the joint variable accelerations can be obtained by solving a 
linear matrix equation 


H(6)6 = (x - db). (42) 


The bias vector b is computed by setting 9, 6 and k to their current 
state, and letting 6 = 0. ARM1 is then called in the following 
manner: ARM (0, 0,0, k, b). Given these inputs, the bias vector b is 
computed in ARM1, 

The difficult part in solving the above linear equation is in 


the matrix H element evaluations. This is done by setting ® to its 


current state. ARM2 is then called in the following manner: 
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DRNARC ie ah Here, °, is a n x 1 vector with the jth element equal 
to 1, all other values of e are equal to 0. The variable a is the 
jth column of H and represents the joint actuator torque when the 
joint velocities are zero. When the H matrix elements aredetermined, 
the joint accelerations can be obtained by solvingequation (42). 
6. Program Flow 
Summarizing, the computational procedures are: 
MAIN PROGRAM (process trajectories and torques) 
SUBROUTINE CONST 
SUBROUTINE TRAJ 
SUBROUTINE ROBARM (Knot #,0, 0,0, ¢, k, PLYLD, t) 
SUBROUTINE KINEMT (6, PLYLD) 
SUBROUTINE ARM1 (0, @,0,,B) 
i= 1 —— 756 
SUBROUTINE ARM2 (6, e,H) 
X=t-B 
SUBROUTINE TDDSOL (H,9,X,error status) 
SUBROUTINE VELSOL (t,0,0,Knot #) 
SUBROUTINE POSSOL (t,0, 0,8, Knot #) 
SUBROUTINE ROBSIM (Knot #,0,0,0,%, k, PLYLD, t) 
where subroutine: 


CONST -— Loads constants, including: radii of gyration, masses, arm 
offsets, differential matrices, gravity constants. 


TRAJ —- Loads trajectory data, either from point-to-point information 
or a fitted curve. Includes time, torque, velocity, and position. 
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ROBARM —- Central umbrella subroutine, called by main program as each 
trajectory point is processed, Variable PLYLD allows program to 
adjust for playload weights and variable t is a time interval (sec). 
KINEMT ~- Calculates the link and transformation matrices, 

ARM1 ~- Subroutine which normally calculates link torques, given link 
position, velocity, acceleration and external forces, however in this 


Simulation method, was used to calculate the inertia matrix terms. 


ARM2 — Same as ARM1, with Coriolis, gravity, and coupling terms 
removed, 


TDDSOL - Linear simultaneous equation solver using trianglur 
decomposition method, 


VELCTY ~ Integration subroutine used to solve for velocity given 
acceleration, using the trapezoidal method. 


POSSOL - Integration subroutine to solve for position using the 
Hermitian form, 


ROBSIM ~- Finite difference simulation subroutine. 
7. Progr lgorithm Development 

; The maximum number of links allowed in this program is 
fifteen (15). Thus, matrix maximum dimensions are configured by row, 
column, and link number. For example, the dimensioning of the link 
coordinate matrix is A(4,4,15). There are no serious limitations 
with these dimensions when using a large capacity computer (i.e. IBM 
3033 system). 

The maximum number of knots which can evaluated is 
arbitrarily set at one hundred (100). This number can be changed 
significantly, without seriously overloading memory allocations, 


The specific numerical values and equations used to develop 


the computer program are provided below. 
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a. Subroutine CONST 
(1) DATA Input. The relative link masses (kg) for PUMA 
ARM 600 are: 


33.5 
11.2 
36.3 
m= 8.95 
2.08 
1.00 


the radii of gyration (cm?) are: 


451.0 451.0 ao 

365.7 1847.0 1408.0 

a 672.8 k2 = 67951: , == 36.0 
xx 31.6 Vy 2 a 2 31.6 
6.9 41.2 6.9 

33.8 33.8 911 


the first moments (cm) are: 


0.0 0.0 8.0 

=2 1°26 0.0 Zura 

=a 0.0 =e 0.0 re 21.6 

0.0 zs 2.0 0.0 

0.0 0.0 0.0 

0.0 0.0 1.0 

and the link parameters for the revolute arm are: 

-90.0 6, 0.0 0.0 
0.0 6. 0.0 43.2 
x 50.0 - | 9; _ {12.5 ~ 1.9 
¢ = |-90.0 : 8, o * 143.2 . 0.0 
90.0 6. 0.0 0.0 
0.0 6 0.0 0.0 
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(2) Calculations. Recalling equation (19), the above 


data is used to compute the Inertia matricies: 


2 2 2 
k. TE eg @ bxa e 2 r 
aS 2b 22 JX k. k. x. 
> jxy jxz j 
2 2 2 
ke +e, 
oe jxx jyy jzz 2 re 
1) aa JxY 2 a J (19) 
2 2 2 
2 2 Kjxx‘iyy “jizz = 
jxz jyz 2 j 
= y. Zz 1 
J yj J 


(3) Programming Philosophy. The physical and geometric 


data is loaded from a disk file by the CONST subroutine. The 
calculation in this subroutine is only called once, as values therein 
remain unchanged during all conditions. The data includes: the 
number of links; the type of link (prismatic=[1] or 
translational=[2]); the Denavit-Hartenberg a, d, a, © parameters, the 
radii of gyrations; the centers of mass; and the link masses. The 0 
(zero) matrix is then loaded and parameter a is converted to radians, 
The O matrix is used in subroutine ROBARM. 

In addition to computing and filling the J; matrix 
and r vectors, the A and T matrices are initialized to zero, The A, 


and I, matrices are filled as identity matrices, 
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b. Subroutine KINEMT 


The link coordinate, first and second 


(1) Calculations. 


partial matrices are calculated using equation (1), repeated below: 


cos @. | -sin @. cos a, | sin @, sina. | a. cos @. 
J, J | J es J 
sin @. | cos 0, cos a. | -cos @. sin a. | a. sin 9 
roe i j i, j Jj, J j (1) 
J 0 | sin a | cos a, | d 
5 j J j 
L4 0 | 0 0 | 1 
-sin 6. | -cos @. cos a. | cos @. sina. | —-a. sin Q. 
J, J J, J Jy J J 
OA : : : 
j cos ©. | -sin ©, cos a, | sin @. sina, | a. cos @ 
mi jy j a j J ia J} 43) 
ae. 0 0 0 | 0 
J \ { ' 
0 | 0 | 0 | 0 
-cos @, | sin @, cos a | -sin ©, sin a, | -a. cos 09. 
J j Jan, j j, Jd j 
07A., ; 
j -sin @, | -cos 6, cos a, | cos @, sina. | -a, sin 6. 
aus jy j I J Jy J (44) 
ae. ? 0 0 | 0 | 0 
J ' , 
Le 0 | 0 | 0 | 0 


Using the above, Le is calculated using equation (2) 


and equation (33): 


J 
Fetgagg eee 


T, 


j = 


The subroutine is called by 


(2) Programming Philosophy. 
ROBARM at each knot. The calculations preformed in this subroutine 


are dependent on the value of link ‘variables’ and change at each 
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knot calling. In the cases discussed in this thesis, all the 
revolute ’’variables’’ are 0. The constant physical and geometric 
data are passed from the CONST to the KINEMT subroutines via a COMMON 
statement (ROB2,ROB3). These parameters are used to compute all of 
the A, @A/00 and 0?7A/d6? matrices for links 1 to 6. As previously 
stated, these and other 4x4 ate ieee are configured in the program as 
by [row, column, link number], 

Upon determination of the A matrices, the values of 


T. 


iL T T,, T-, and T. are computed using Equation (2). 


wee 3 || 4 CSS 6 
Finally, the gravity vector is loaded so that gravity 
acts in the positive Z direction, All these vectors and the above 
matrices are passed to other programs via a COMMON statement 
(ROB8,ROB9). 
c. Subroutine ARM1 
(1) Calculations. Using equation (34) and equation 


C35), T,, T, and T, are computed from j=1 to j=6: 


® e OA. 





T=T A +T.. —o@. 45 
j j-i Jj Jl 5g va 
j 
e680 @¢ @ GA e aa e oA, ee 
T. =T. , A, +2T., —06, + T,_ 62 +T. . —— 6. (46) 
j jal - J j-1l 59 j J-1 562 Jl 5g J 
i j 
Additionally, oT, GA. 
SS ye (47) 
30 JI” ae, I 
j j 
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When T., T., and T. are computed, D. and c. is 
J J J a i 


computed from j=6 to j=l using equation (37) and equation (38): 


e¢¢ j 
a se T; A ies eed °; eed 
where: 
0 x, 0 
1. ue 0 2. zs 0 3. Z 0 
1 Zs 2 Zs 3 Zs 
1 1 1 
0 0 0 
Aeon Sa ae) 0 6 _| 0 
t4 0 ts 7 Zs <6 7 Ze 
1 1 1 


The torques in a revolute manipulator arm are then computed by 


rewriting equation (39): 


oT. aT 
c. = tr| —Jp. - eo c, (48) 
J 0, J 0. ‘ 


(2) Matrix Multiplication and Addition Subroutines, The 


following subroutines are called during ARM1 and ARM2 to perform 
repeated multiplication and additions on indexed 4x4 and 4x1 
matrices: 


MAT44 —- Multiples a 4x4 matrix and a 4x4 matrix 

MAT44T - Multiples a 4x4 matrix and a 4x4 Transposed matrix 

MATC4 - Multiples a Constant and a 4x4 matrix 

MATC1 - Multiples a Constant and a 4x1 matrix 

MAT41 ~ Multiples a 4x4 matrix and a 4x1 matrix 

ADD44 - Adds a 4x4 matrix to another 4x4 matrix 

ADDALL - Adds 4 different 4x4 matrices 

ADD11 ~ Adds a 4x1 matrix and a 4x1 matrix 

FIRST - Finds the trace of two multiplied 4x4 matrices to find 

the left (first) side of the force equation (48). 

SECOND - Multiplies a 1x4 matrix and a 4x4 matrix, and 
subsequently another 4x1 matrix to find the right 
(second) side of the force equation (48). 
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(3) Programming Philosophy. ARM1 and ARM2 contained many 
short subroutine calls which are used to multiply and added matrices. 
A variety of real constant arrays are declared for tse in the ARM1 
and ARM2 programs (C1 to (C15). These area provided to '‘hold’’ 
preliminary calculated data for further use by other subroutines. 
The use of many rather than few holding matrices is based on the lack 
of memory constraints, and the avoidance of the need to constantly 
reinitialize. The use of these holding matrices can be avoided by 
rewriting the matrix multipling and adding subroutines so there is no 


requirement for holding. 


The first terms calculated are T, from j=1 to 6, 
When j=1, the left side of equation (48) vanishes leaving only 


the right side, This condition is satisfied using an '’IF’’ loon, 


Similiar equation '’reduction’’ conditions exist for 


the T, D C., and oT ,/00. matrices. The D. and C, matrices are 


i i 


filled in reversed order (backward recursion). 


d. Subroutine ARM2 


(1) Calculations. Rewriting equation (35), T, is 


computed from j=l to j=6: 


® @ e ¢ OA. ee 
T =T. A +T,. —o 49 
eee j ee) 
a0 
® 8 J 
When T;. i are computed, D. is computed from j=6 to j=l using 
equation (38): 
D = J.T+A.__D. 
J jae, Jt ee +d 
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The torques are finally computed using: 


oT. 
ct, = tr —1 p. (50) 
J ae, J 
J 


(2) Programming Philosophy. The memory requirements for 
ARM2 are less, since the velocity and gravity terms are removed, 
Furthermore, there is no requirement for calculation of T, since 
T, is 0, and all velocity terms are removed, leaving all subsequent 
terms to equal 0. 

Additionally, the Cc. terms are not required as they 
are used with gravity terms in the force equation, The oT ae 
matrices are calculated in ARM1l and their values remain unchanged 
from those used in ARMI1. 

e. Subroutine TDDSOL 

The subroutine TDDSOL is an IMSL simultaneous equation 
solver called LEQIF. This subroutine provides a solution to linear 
equation generated by the inertia and bias matrices, using full 
matrices in virtual memory. The program listing is not provided for 
this copyrighted material (1982, by IMSL, INC.). The subroutine 
calls other IMSL subroutines. However, any linear equation solving 
subroutine, with capabilities to warn of matrix singularities can be 
used, Argument input for this subroutine was: 


Argument # —- Input n x n ’A'-matrix of equation AX=B 


1 

2 - Rows in 'A’ as dimensioned by calling program 
3 - The order of matrix ‘A’ 

4 - Equal to n (related to program speed) 

5 Right hand side of equation AX = B, 

at output, solution matrix X replaces B. 

6 -— Rows in ‘'B’ as dimensioned in calling program 
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- Number of columns in ‘'B’ 
- I=0, Code to factor matrix and solve equation AX=B 
~ Real work area = 3¥*n 

O - Error parameter IER=129 —> Matrix a singular AX=B 


19 6 ~] 


f. Subroutine VELSOL 
Subroutine VELSOL was written as a simple adaption of 
trapezoidal rule [Ref. 473 page 75]. The subroutine provided the 
velocity solution by integrating angular acceleration over the entire 
time considered, The argument input for this subroutine was: 
Argument # 1 - Time, all time point covered to point of calling 
2 ~ Acceleration, submitted one link a time 
3 - Velocity, output 
4 - Link number 
ge. Subroutine POSSOL 
Subroutine POSSOL is written using an adaption of a 
Hermite interpolation method of integration [Ref. 473 page 314]. 
This method provides a more accurate solution as derivatives are 
known. The argument input for this subroutine is: 
Argument # 1 -— Time, all time point covered to point of calling 
~ Acceleration, submitted one link a time 
Velocity, from previous subroutine 


Position, output 
~ Link number 


th & W bo 
( 


h. Subroutine ROBSIM 
This subroutine implements the simulation technique 
described in section III.C.3. The simulation program is called by 
setting ISIM equal to one (ISIM=1) in the main program (ROBOT). When 
this subroutine is called, all output can be sent, via a separate 
subroutine, to the same position, velocity, and acceleration files as 


the modelling program. These files are for use with graphics 
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programs, This subroutine computes new position and velocity 
information, and calls ARM1 and TDDSOL to determine the new 
acceleration at each step. Step sizes are determined by setting the 
number of steps in the main program (ISTEP = number of steps). 

Torque is keep constant as the subroutine is called at each knot. 

Arguments passed to this subroutine are identical to those passed to 
ROBARM. Only the knot number, torque, and the time matrix are used 
throughout an entire program run. Arguments for position and 


velocity are used only on the first knot. 


D. DATA INPUT 

Discrete torque data is fed into the modelling and simulation 
process at each trajectory knot. An analysis of the relative joint 
moment magnitudes (except friction) was performed for the PUMA 
manipulator by R.P. Paul [Ref. 46]. Paul estimated the values for 
the link masses and radii of gyration based on a manipulator 
examination. The actual mass and radii of gyration values were 
obtained analytically, and were within an estimated 10% of actual 
values. Paul’s results were used as input data for this simulation, 
This input data includes all robot link inertias, motor torque 
characteristics, and accuracies. The torque inputs and 
velocity/positional output data was obtained through use of feedback 


controller sensors, and its accuracy is estimated with 1% to 2%, 


64 


E. PROGRAM OUTPUT 

Simulation program output included the position, velocity, and 
acceleration versus time data for the end effector and each joint. 
This data was placed in a data file which easily interfaced with 
a graphics interpreter program, Data output included: 

Desired Position vs Computed Position files (PEn DATA) 

Desired Velocity vs Computed Position files (VEn DATA) 

Desired Acceleration vs Computed Acceleration files (AEn DATA) 

Error evaluation files (ERROR DATA) 

Additionally, an independent subroutine (TRANSP) was developed to 
calculate the end-effector transposition matrix using the actual and 
computed position files. The transposition matrix was then used as 
output for this subroutine, providing the x,y,z coordinates of the 
end-effector. Data output included: 

Actual path x,y,z coordinates (PATHACT DATA) 

Modelled path x,y,z coordinates (PATHMOD DATA) 

Simulation path x,y,z coordinates (PATHSIM DATA) 


This data was used to provide a graphical representation of the 


end-effector path. 
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IV. RESULTS AND DISCUSSION 


A. MODELLING PROGRAM RESULTS 

Based on the dynamic model formulation presented in this thesis, 
a general computer program package for the dynamic analysis of a 
revolute six joint manipulators was developed. 

The input torque for each joint was supplied as a series of 
trajectory knots, as shown in Figure 9 for a typical joint. The only 
arm external loading terms considered were those of gravity. Angular 
position, velocity, and acceleration, were subsequently computed and 
plotted against actual knot values to verify the accuracy of the 
model, as shown typically for joint 2 in Figures 10, 11, and 12 (A 
complete set of all graphs for torque, acceleration, velocity, and 
position are available in Appendix B, Figures 19-42.) 

Table I provides a numerical average modelling error for each 
link for the ceaeetort terms, This error was obtained ~ by 
determining the difference between the actual data and the model 
results, and then normalizing with the actual data. The errors were 


summed over all knots and averaged. 


Table I. Modelling Joint Position Errors 
LINK NUMBER 1 Z 3 4 5 6 


Acceleration | 2.778% 1.492% 2.201% 1.244% 2.543% 2.510% 
Max Error (-/sec?) | 0.152 0.494 1.542 0.103 1.222 0.552 
Velocity | 6.457% 4.404% 3.091% 2.824% 5.397% 4.283% 
Max Error (°/sec) | 2.306 1.267 2.050 1.487 3.055 1.687 
Position | 7.210% 8.156% 5.979% 9.640% 13.386% 5.763% 
Max Error (°) | 10.871 8.154 13.926 12.968 13.899 8.170 
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After all trajectory knots had been analyzed by the main program, 
the predicted joint model positions were supplied to another 
independent computer program TRANSP (similiar to KINEMT), where the 
angular positions of each link were transposed, from base to end- 
effector, to determine the position vector of the end-effector, The 
motion produced in each plane, XY, XZ, and YZ, was then plotted 
graphically, as seen in Figures 13-15. Table II. provides the 
average error achieved while the arm moved over the specified path. 
Again, this error was obtained by determining the difference between 
the actual data and the model results, normalizing with the actual 
data, and again averaging over the range of knots, The errors 


translate to a maximum path variation of approximately 12 cm. 


Table II. End-effector Modelling Errors 




















Knots # X coord Y coord Z coord 
Avg Error 22.72% 12.25% 11.08% 
Max Error 27.4cm 4.1cm 7.1cm 
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B. SIMULATION PROGRAM RESULTS 

The use of the simulation subroutine was bypassed (ISIM=0) during 
the above operations. This was done to allow modelling without the 
the simulator program. When the simulation subroutine was 
incorporated into the program (ISIM=1), several simulations, using 
varying stepsizes, were evaluated for the series of knot points. 


Torques were held constant during simulations between knot points. 
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In addition to noting errors achieved during each evaluation, the 
time required to run the simulator program on the IBM 3033 was also 


recorded. This information is provided in Table III and Figure 16. 


Table III. CPU Time Required to Process Simulation Program (Seconds) 














Knots Number of Steps Processed between Knots 
Processed 5 10 15 20 25 30 
ee ee. 
3.0 | 1.53 3.03 3.43 6.01 6.37 9,06 
5.0 | 1.84 4.08 6.65 9.61 12.10 17.30 
10.0 3.34 8.22 14.10 20.20 26.40 37.40 
15.0 | 4.57 12-20 20.20 30.60 40.50 60.30 
20.0 | 5.74 17.50 26.80 38.30 50.80 79.10 
25.0 | Tel 20.20 33.00 48.00 63.00 99.20 
30.0 | 8,43 24.00 39.40 57.30 75.20 125.20 
35.0 | 9,69 28.50 45.40 68.50 88.00 138.20 
37.0 


10.40 29.80 48.30 71.50 94.30 152.30 








Once again, acceleration, velocity, and position outputs were 
obtained and the results were recorded graphically, as shown in 
Appendix B. The trajectory information for joint 1 is provided in 


Figures 17-19, other results are included in Appendix B. 


C. DISCUSSION 
1, Model 
The most important result achieved was that the program 
demonstrated the capabilities to generate a forward dynamic solution 
and serve as an accurate dynamic robot model. Linear programming 
techniques were used to determine the angular acceleration. This 
program was successfully tested against known results for a given 


trajectory of the PUMA arn, 
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The acceleration data supplied was determined analytically; 
however, the method used to determine this data was not available. 
The small acceleration errors achieved gives rise to a suspicion that 
the analytical process may also have included joint torque 
information, possibly processed through a dynamic formulation program 
similar to that presented in this thesis, 

The accuracy of the data which describes a robot's joint 
links, mass, to a set of general inverse kinematic equations was 
estimated at +10% [Ref. 46]. The physical property data was 
Originally obtained by ‘'‘'fitting’’ the manipulator arm’s mass and 
inertia terms to a point where they are consistent with the robot's 
motions, The error achieved by the model appears to be well within 
results which could be expected from the data provided, 

It was noted that the model end-effector path tracked 
erratically in all Knots after 15 seconds, The source of these 
errors can be seen by inspecting the angular position verses time 
figures for each joint, and observing that the individual joint 
errors increase at times after 15 seconds, Although at worst these 
errors are relatively small (7-8%), they accumulative towards the 
end-effector. This is due to the nature of process involved in 
transposing end-effector coordinates to base coordinates. 
Consequently, this produces a 12-13% error in end-effector position, 
This error seems reasonable, considering the low magnitude of 


individual joint errors. 
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The thesis display program (TRANSPLT) was capable of 
displaying motion data in a two dimensional plane and provided an 
acceptable, but limited method of visualizing robot motion in order 
to evaluate robot performance, 

2. Simulation 

The accuracy of the 6 link simulation program deteriorated 
rapidly after five seconds of simulated performance. At that time 
the accelerations oscillated rapidly about the actual values, with 
magnitudes greater than 500%. Velocity and positional results were, 
consequently, several orders of magnitude different than expected 
results, In an effort to eliminate the possibility that excessive 
errors in kinematic variables near the end-effector were creating 
large errors in the first three links, the masses of links 4, 5, and 
6 were combined arithmetically, and the program was rerun as a four 
link problem. While acceleration results did not change under this 
second evaluation trial, velocity and positional data were generally 
as expected for the first 5 seconds. Typical velocity results are 
shown in Figure 18. The remainder of the disscussion will address 
the second (four link) evaluation, 

A detailed examination was made to determine the nature of 
the oscillations for a segment between two knots. At the beginning 
of each segment, a new torque value is supplied to the simulator. 
This torque value does not change further until the next segment is 
evaluated. While velocities and positions form an integral relation 


with accelerations, they are not treated as such in the non-iterative 
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constant acceleration approximation scheme implemented in the thesis 
simulation routine. Consequently, as each succeeding step is 
evaluated, the method produces velocities and positions which become 
increasingly more inaccurate as the time evaluation proceeds. Since 
these values are supplied to the matrix equation for. the 
acceleration, it can be seen that the acceleration errors will 
iwerense as velocity and position errors compound. 

Values in the inertia matrix, which are supplied as the left 
side of the matrix dynamic equation, were examined at all evaluation 
points in the belief that the deviations in acceleration may have 
resulted from singularities produced in a sparsely occuppied matrix. 


However, singularity conditions were not observed. 
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V. CONCLUSIONS 


A. MODEL 

A computer program was written which allowed the calculation of 
open spatial mechanical manipulator chain dynamics based on the 
application of independent equations represented in Hollerbach’s 
recursive Lagrangian forn, The resulting dynamic formulation was 
developed in numerical matrix form rather than as specific equations 
of motion. The model was evaluated for the PUMA robot manipulator. 

In view of the inaccuracies in model constants it is difficult to 
evaluate the accuracy of calculated variables. However, the model 
did produce results of sufficient accuracy that it could provide 
insight into the dynamic properties of the manipulator arm, and could 


give assistance in the task of trajectory planning. 


B. SIMULATION 

The simulation routine did not produce reliable or accurate 
results due to the programs’ inability to continuously provide values 
of velocity and position which would satisfy their integral relation 
with acceleration. The linearizing approximation of stepwise 


constant acceleration was a poor approach. 
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C. FUTURE WORK 
1. Program Enhancements 

The simulation routine must be rewritten to iterate about 
each acceleration step until accurate values of velocity and position 
are obtained for the next step. 

An entire robot arm system can be modelled by: (1) modelling 
of an operational amplifier which can convert the position error 
signal into a current to drive a servo valve, (2) modelling a 
hydraulic actuator system (which can be a second order model for a 
flow control valve, and an integrator to model the actuator), and (3) 
connecting the output of the hydraulics system (torque) to the input 
for the thesis model in order to predict joint accelerations, 
velocity, and position. 

The robot modelling could be modified to accommodate other 
manipulators with Euler or cylindrical coordinates. Additionally, 
although the discussion in this thesis deals primarily with open 
chain systems, the program can be readily extended to treat closed 
loop systems. 

2. Computers 

Future enchancements of the thesis program could include 
better user interfacing (’'’user friendly’’), more efficient code, 
better utilization of memory, and improved input/output. 


Additionally, three dimensional kinematics representations be 
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designed by interfacing the program with CAD/CAM or finite element 
software. 

Microcomputer (Personal Computer) solutions to applications 
problems will become a common practice among manufacturing engineers 
in the coming years. It is recommended that the FORTRAN program be 
compiled and run on a microcomputer, which could ultimately can be 
used to control an actual manipulator arm. Memory allocation might 
become a serious problem if this thesis program is transferred to a 
smaller micro~ or mini~computer system. A benefit of using a 
dedicated mini~ or microcomputer would be the decreased dependence on 
time-shared mainframe computer time. 

Additionally, the program can be modified for double 
precision to increase accuracy, particularily around suspected 
singularity points, at an expense of increased total computation 
time, 

36 sical sis 

The physical and geometric parameters of robot arm components 
are perhaps the most difficult to obtain. In addition to published 
data, generalized methods are available which can be used to obtain 
these values, The program can be used to analyze the physical 
parameters of small laboratory manipulator arms by varying dynamic 
parameters estimates to match Known motion data. Such a model can be 
coupled directly to a robot arm control programs, where results for 
each of the major arm linkages and control components can be 


validated experimentally. 


85 


The modelling program can be used to evaluate the relative 
effects of acceleration and velocity on various terms in the dynamic 
equations. 

4. Control Design 

In its present form, the program permits development of robot 
and workspace models, robot motion programming, and duty cycle 
calculations, such as forces, torques, and deflections. 

When using the program to design workcells by producing two- 
dimensional diagrams of manipulator motion, work points can be 
defined, and a motion path can be analyzed to allow collision 
avoidance evaluations, 

The simulation can be used to determine the effects of 
variables on dynamic performance, to develop robot dynamic analysis 


data, and to identify and evaluate robot singularity conditions. 
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APPENDIX A 


RULES GOVERNING LINKS, JOINTS, AND THEIR PARAMETERS 
1. Mechanical manipulators consist of a sequence of rigid bodies, 
called links, connected by either revolute or prismatic joints. Each 
joint-link pair constitutes one degree of freedom. 
2. An nm degree-of-freedom manipulator has n joint-link pairs with 
link 0, Link 0 is not considered part of the robot 
3. Link 0 is attached to a supporting base where an inertial 
coordinate frame is established. Last link can have a tool. 
4. Joints and links are numbered outward from the base 
5. Each link is connected to two others at most so that no closed 
loops are formed. 
6. A joint axis (for joint i) is established at the connection of 
two links. This joint axis has two normals connected to it, one for 
each link, 
7. The relative position of two such connected links (link i-1 and 
link i) is given by d.» the distance measured along the joint axis 
between normals, 


8. The joint angle, 0 between the normals is measured in a plane 


i’ 
normal to the joint axis. They determine the relative position of 


neighboring links, 
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9. A link i is connected to two other links, 
Two joint axes are established at both ends of connection. 
Links maintain fixed configuration between their joints. 
Configuration structure is characterized by a. and a 
d. and 6. determine the neighbor link relative positions 
10. There are aio Gs, d. and 6. parameters associated with each 
manipulator link and they represent the minimal sufficient set to 
determine complete kinematic configuration of each robot arm link, 
a, = length, shortest distance along common normal between joint axes 


a. = twist, angle between joint axes in plane perpendicular to aie 
d. = distance between adjacent links. 
6. = angle between adjacent links, 
DENAVIT-HARTENBERG REPRESENTATION 

The Denavit-Hartenberg representation is a matrix method of 
systematically establishing a coordinate system (body-attached frame) 
for each link of an articulated chain. A 4x4 homogeneous 
transformation matrix represents each link'’s coordinate system at the 
joint with respect to the previous link’s coordinate system, 

An orthonormal Cartesian coordinate system is established for 
each link at its joint axis, plus the base coordinate frame. Since a 
rotary joint has only one degree of freedom, each robot arm 


coordinate frame corresponds to joint (itl) and is fixed in link i. 


When an actuator activates joint i, link i moves with respect to link 
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(i-1). Since i’s coordinate system is fixed in link i, it moves with 
link i. 
The nth coordinate frame moves with the tool or hand, (link n). 
The base coordinates are defined as the Oth coordinate frame 
(x09 5 925) which is also the inertial robot arm coordinate frame. 
Every coordinate frame is determined and established by three 
rules: 
1, the z,_, axes lies along the motion axis of ith joint 
2. the x. axis is normal to the z -1 axis, pointing away 
3. the V3 axis completes the Pint hand coordinate system 
The location of coordinate frame 0 may be choosen anywhere in 
supporting base, as long as the Zo axis lies along the motion axis of 
first joint. The nth frame can be placed anywhere in hand as long as 
the x axis is normal to the z axis. 
n n-1 
The Denavit-Hartenberg representation of rigid links depend on 


four geometric quantities associated with each link that completely 


describe any revolute/prismatic joint. 


1, 9. = joint angle from X,_, axis to x. axis about Zod 
axis (Right Hand Rule) 

Zs d. = distance from (i-1)th coordinate frame origin to 
intersection of Zs 4 axis with the x, axis along 
the 254 axis. 

Die a. = offset distance from intersection of z,_, axis with 
x, axis to origin of ith frame along x. axis (or 
shortest distance between Z4 and Zz, axes, 

4. a6 = offset angle from z._, axis to zs axis about x, 
axis (Right Hand Rutet 
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For a rotary joint, d.. ais and a. are joint parameters and 
remain constant for a robot. 6. is a joint variable that changes 
when link i moves (rotates) with link i-1. For a prismatic joint, 
6» as and a, are joint parameters and remain constants for a robot. 


d. is joint variable. 


PROCEDURE FOR ESTABLISHING CONSISTENT ORTHONORMAL COORDINATE SYSTEMS 
Given: m-degree-of-freedom robot arm 


Establish: an orthonormal coordinate system to each robot arm link, 


1. Establish base coordinate system. 
Establish a righthand orthonormal coordinate system (xX +¥ 5°25) 


at supporting base with Zo axis lying along joint 1 motion axis. 


For each i, i=1,...n, perform steps 3-6 


3. Establish joint axis. 


Align Zz; with joint itl motion axis 


4. Establish origin of ith coordinate system. 


Locate origin of ith coordinate system at intersection of Z. and 
Zi1 axes or at interestion of common normals between Zs and Z 84 


axes and Zz. axis. 
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5. Establish x. axis. 


H(z.) x z) 





Establish x 
Iz, x 2,1 

or along common normal between Z-4 and zZ, axes when they are 

parallel. 


6. Establish y. axis. 
i = 
rm (z. x =.) 
. i i 
Establish y; = -—- + 


to complete righthand coordinate system. 


7. Find joint and link parameters. 


For each i, i=1,...,m, perform steps 8-11. 


8, Find d.. 
1 


d, is distance from (i-1)th coordinate system origin to 


i 
intersection of Z sy axis and x; axis along Z 4 axis. It is 
joint variable if joint i is prismatic. 
9, Find a.. 
i 
a; is distance from intersection of Z 4 axis and x. axis to ith 


coordinate system origin along x. axis, 


10. Find 9.- 


8. is rotation angle from x, _ axis. 


axis to x. axis about z. 
ivl 1 - 


i=] 
It is joint variable if joint i is rotary. 


11, Find a. 


a. is rotation angle from Zsi4 axis to Zs axis about x, axis. 
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Once the Denavit-Hartenberg representation coordinate system is 
established for each link: 


A Homogeneous tranformation matrix, A can be developed 


ie 


(Relates ith coordinate frame to (i-1)th coordinate system) 


1. rotate about z._, axis an angle of 6; to align x,_ 


ik i= 
axis with x, axis (x,_, axis is parallel to x,) 
2. translate along Zod axis a distance of d, to bring 
Xi-1 and x, axes into coincidence, 


3. translate along x, axis a distance of a. to bring 
two origins into coincidence 
4. rotate about x, axis an angle of a to bring the two 
coordinate systems in coincidence, 
Each operation is expressed by a basic homogeneous rotation or 
translation matrix, and the product of 4 basic homogeneous 


transformation matrices 


This yields composite homogenous tranformation matrix, Ais 
known as the Denavit~Hartenberg transformation matrix for adjacent 


coordinate frames, 
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APPENDIX C 


A. COMPUTER PROGRAMS 


CFSSSSSSESSSSSESSSESSSSSSESSESSSESSESSSSESSSSSSSESSSSESSESESSSSE SSE SSE SSS SESE SMATN 
CFSSSESSSSSSSSSSESSSSSSSSESSESSSESSSSESSESSSESEKSESESSE CSE SELES SE SESE SE SESE SENMATN 
CFSSSSSSSSSSSSSSSSSSSSSSSSESSSESSSSSKSSSSSSSESHSESSESSSSSKSESSEKSESSESSSELEVATN 


COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 


2LD 


/ROB1/ 
/ROB2/ 
/ROB3 / 
/ROB4/ 
/ROBS/ 
/ROB6/ 
/ ROBT / 
/ROB8/ 
/ROB9/ 


G,PI,D2R,R2D 

ERTIA(4,4,15) ,RBAR(4,15) 
SMALLA(15) ,SMALLD(15) , ALPHA(15) 
B2XX(15) ,B2YY(15) ,B2ZZ(15) 
AMASS(15), ACTIA(15) , LINKS 
XBAR(15) , YBAR(15) ,ZBAR(15) 
GRAV(4,15), LNKTYP(15) 
A(4,4,15),PA(4,4,15),PPA(4,4,15) 


/ROB10/ POS(15,100) , VEL(15,100) ,ACC(15,100) 

/ROB11/ DELTA(15,100) , DELTV(15,100) , DELTP(15,100) 

/ROB12/ SIMERR(15), TOTACC(15) , ACCSIM(15,100), ISTEP, KNOTS 
/ROB14/ POSSIM(15,100),ISIM 

REAL T(15),TD(15),TDD(15) , TAU(15) , FMK(15) , THETA(15,100) , THETD(15,1 
100) , THDT2(15,100) ,TU(15,100) , TIME(100) , TS(15) ,TDS(15) , TDDS(15) , PAY 


INTEGER I,J,LINKS 


ISTEP=5 


IS IM=1 


CALL CONST 


a a LOAD TRAJECTORY KNOTS 


CALL TRAJ (KNOTS, TIME, THETA, THETD, THDT2, TU) 
aa.) BNI TIALIZE PASS®ON MATRICES 
DO 10 I=1,15 


Z(1)=0. 


0 


TD(I)=0.0 
TDD(I)=0.0 


FMK(I)= 


0.0 


CONTINUE 


DO 30 J=1,KNOTS 





PAYLD=0.0 
DO 20 I=1,LINKS 
FMK(I)=0.0 


TAU(I)=TU(I,J) 


—-----CALL SUBROUTINE TO COMPUTE ACCELERATIONS 


T( I) =THETA(I,J) *D2R 

TD( I)=THETD(I,J) *D2R 
TDD( I) =THDT2(1I,J) *D2R 
CONTINUE 
CALL ROBARM (J,T,TD,TDD, TAU, FMK, PAYLD, TIME) 
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CALL OUTPUT (J,T,TD,TDD, THETA, THETD, THDT2, TIME, PAYLD) 
30 CONTINUE 


en i OL, 
CALL ERRORS (KNOTS, THETA, THETD, THDT2) 
¢-——__-_-—__--____________________________.__-__ FIND SIMULATION ERRORS 
STOP 
END 


CFSSSSSSSSSKSSTSSSKSKSESTETKTKTETTTTKESTTT ERLE’ SKERESSESTESEESEESESSESESROBARM 
C#SSSSSSSSTSSTTSTTTSKVETTS VETTES TEESE TEE ESTETS EEE ESSEROBARM 
CFSSSSSSSSSSSESSTSSTSEKSTSEKTKLESTKEKERERELERERERERENES HEE SEESEEEESESROBARM 


10 


C--- 


20 


30 


50 





SUBROUTINE ROBARM (KN,T,TD, TDD, TAU, FMK, PAYLD, TIME) 


COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 


/ROB1/ 
/ROB2/ 
/ROB3 / 
/ROB4/ 
/ROBS / 
/ROB6/ 
/ROB7/ 
/ROB8/ 
/ROB9/ 


G,PI,D2R,R2D 

ERTIA(4,4,15) ,RBAR(4,15) 
SMALLA(15) ,SMALLD(15) , ALPHA(15) 
B2XX(15) ,B2YY(15) ,B2ZZ(15) 
AMASS(15), ACTIA(15) , LINKS 
XBAR(15), YBAR(15),ZBAR(15) 
GRAV(4,15) , LNKTYP(15) 
A(4,4,15),PA(4,4,15), PPA(4,4,15) 
ZERO(15),TT(4, 4,15), PTQ(4,4,15) 


/ROB10/ POS(15,100),VEL(15,100) ,ACC(15,100) 
/ROB14/ POSSIM(15,100), ISIM 
REAL T(15),TD(15),TDD(15) , TAU(15) , FMK(15) , HJ (15) ,B(15),ES (15) ,H(15 


1,15) ,WORK(45,3), TIME(100) , TDOT2(100) , TDOT(100) , TPOS(100) , PAYLD 


INTEGER I,J, ISTAT, KN 
CALL KINEMT (T, PAYLD) 
DO 10 I=1,15 


B(I)=0. 


0 


CONTINUE 
CALL ARMi (T,TD,ZERO, FMK,B) 


—---~——-—- +--+ -——— -——_——-——-LOAD EJ VECTOR 


DO 40 I=1,LINKS 
DO 20 J=1,LINKS 
HJ (J )=0.0 
EJ (J)=0.0 
CONTINUE 
EJ (I)=1.0 
CALL ARM2 (T,EJ,HJ) 
DO 30 J=1,LINKS 
H(J,1)=HJ (J) 
CONTINUE 
CONTINUE 

DO 50 I=1,LINKS 
TDD( I) =TAU( I)-B( I) 
CONTINUE 


IER=0 


Cn rr nn COMED hss Linnie n c Chien ndemcens 
CALL TDDSOL (H,15,LINKS,LINKS,TDD,15,1,0,WORK, IER) 
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(------------~~—----------~-~-----------L0AD VECTORS INTO PERM MATRICES 
DO 80 LN=1,LINKS 
TDD(LN) =TDD(LN) *R2D 
ACC(LN, KN) =TDD(LN) 
DO 60 I=1,KN 
TDOT2( I) =ACC(LN, I) 
TDOT(I)=0.0 
60 CONTINUE 
CALL VELSOL (TIME, TDOT2, TDOT, KN) 
(----~—---~-------------------------------—- CALCULATE VELOCITY 
VEL(LN, KN) =TDOT( KN) 
TD(LN) =TDOT( KN) 
DO 70 I=1,KN 
TDOT( I) =VEL (LN, I) 
70 CONTINUE 


c CALL POSSOL (TIME, TDOT, TDOT2,TPOS, KN) 
CALL VELSOL (TIME, TDOT,TPOS, KN) 
i a. CMCALCULATE 6 POSITION 


POS (LN, KN) =TPOS( KN) 
IF (KN.EQ.1) TINIT=T(LN) *R2D 
T(LN) =TPOS (KN) +TINIT 
80 CONTINUE 
a a CALL SIMULATION 
IF (ISIM.NE.1) GO TO 90 
CALL ROBSIM (KN,T,TD, TDD, TAU, FMK, PAYLD, TIME) 
90 CONTINUE 
RETURN 
END 
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C#KKKKAKEKREKEKREREKRAESKAREREAASKAAAAAKAKAAKAAAERAAHARAKAKAKAAAKAKREKLALAARNY 
C#KKKKERKKAKKKAAKEKAKAKKAKEKAHKAKAAKAAKAAAKAEAAKKAAAKAKAEKAKKAKAAEEEEKESE*KARNMI 
CRKKKKKREKRKEKAKAKEKAAKAEKRAKREKREKRAKAKRAAKAKRERAKKKRAKEAAKRERAKRKAEAAKAKARARAKKKARNMT 


SUBROUTINE ARM1 (T,TD, TDD, FMK, TAU) 


COMMON /ROB1/ 
COMMON /ROB2/ 
COMMON /ROB3/ 
COMMON /ROB4/ 
COMMON /ROBS/ 
COMMON /ROB6/ 
COMMON /ROB7/ 
COMMON /ROB8/ 
COMMON /ROB9/ 
REAL T(15) , TD( 
1,4,15),C2(4,4, 
2,4,15),C8(4,4, 
313(4,4,15),C14 
INTEGER I,J 


| Ghlessneeientantantennesmentenmenestantiieeiemen 


10 


30 


DO 30 I=1,4 
DO 20 J=1,4 
Cc(1I,J)=0.0 
C14(I,J)=0.0 
C15(I,J)=0.0 


G,PI,D2R,R2D 

ERTIA(4, 4,15) , RBAR(4,15) 

SMALLA(15) ,SMALLD(15) , ALPHA(15) 
B2XX(15) , B2YY(15) ,B2ZZ(15) 

AMASS(15) , ACTIA(15) , LINKS 

XBAR(15) , YBAR(15) , ZBAR(15) 

GRAV(4,15) , LNKTYP(15) 

A(4,4,15) ,PA(4,4,15) , PPA(4, 4,15) 

ZERO(15) ,TT(4,4,15) , PIQ(4, 4,15) 

15) ,TDD(15) , TAU(15) , FMK(15) , DD(4,4,15) , CC( 4,15) , C1(4 
15) ,C3(4,4,15),C4(4,4,15),C5(4,4,15) ,C6(4,4,15) ,C7(4 
15) ,C9(4,4,15),C10(4,4,15), C11(4,4,15) ,C12(4,4,15),C 
(4,15) ,C15(4,15),C17,C19,TD1(4,4,15) ,TD2(4, 4,15) 





INITALIZE ALL CONSTANT MATRICES 


DO 10 K=1,LINKS 


DD(I,J,K)=0.0 
TD1(1I,J,K)=0.0 
TD2(1I,J,K)=0.0 
C1(I,J,K)=0.0 
C2(1,J,K)=0.0 
C3(1,J,K)=0.0 
C4(1,J3,K)=0.0 
C5(1I,J,K)=0.0 
C6(1I,J,K)=0.0 
C7(1,J,K)=0.0 
C8(I,J,K)=0.0 
C9(I,J,K)=0.0 
C1i0(I,J,K)=0.0 
C1i1(1I,J,K)=0.0 
C12(1I,J3,K)=0.0 
Ci3(1I,J,K)=0.0 
CONTINUE 
CONTINUE 
CONTINUE 
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rT = T 
J 
Cc = 
C TD1/TD2 
Ceo eo 





PA ° 
Ay Git -——J @Q 
eae od J-1 PQ J 
J 
C1 ts C3 


MATRICES INITIALIZED TO ZERO IN CONST SUBROUTINE 


SSS SS ee SG GS SS a ee ee eS ee eee a SS ee eS SE SS SE EE SE SES ee SSeS Se 





DO 60 J=1,LINKS 
IF (J.EQ.1) GO TO 40 
CALL MAT44 (TD1,J-1,A,J,C1,J) 
CALL MAT44 (TT,J-1,PA,J,(2,J) 
CALL MATC4 (TD(J),C2,J,C3,J) 
CALL ADD44 (C1,J,C3,J,TD1,J) 
GO TO 50 
40 CONTINUE 
CALL MATC4 (TD(J),PA,J,TD1,J) 
30 CONTINUE 
60 CONTINUE 








C °° 

C MT ae Teena aT es ae eee LOAD T MATRIX 
C 2 

Car .. . . PA . PA we PA ee 
aan = TT A +2 T ———) Oe rs eee <r rer °Q 

Cc J Jet. Bao J-1 PQ Jj Jal RO. Ze J-1 PQ J 
C J J J 

' Cctensteeteestenteesientententententementententenent oe ce ce re ee ew ee ee ew ee we er es mr we ee ee 
C = C4 + Ci = C9 + C11 

C----- a ee = ee eee 


DO 90 J=1,LINKS 
IF (J.EQ.1) GO TO 70 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


MAT44 (TD2,J-1,A,J,C4,J) 

MATC4 (2.0,TD1,J-1,C5,J) 

MAT44 (C5,J,PA,J,C6,J) 

MATC4 (TD(J),C6,J,C7,J) 

MAT44 (TT,J-1,PPA,J,C8,J) 

MATC4 (TD(J) **2,C8,J,C9,7) 

MAT44 (TT,J-1,PA,J,C10,J) 

MATC4 (TDD(J),C10,J,C11,J) 

ADDALL (C4,J,C7,3,C9,3,C11,J,TD2,J) 


GO TO 80 
70 CONTINUE 


CALL 
CALL 
CALL 


MATC4 (TD(J) **2,PPA,J,C9,J) 
MATC4 (TDD(J),PA,J,C11,J) 
ADD44 (C9,J,C11,J3,TD2,J) 


80 CONTINUE 
90 CONTINUE 
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(an a CO Te 
DO 140 II=1,LINKS 
I=7-II 

(-~-------------------—--------—- LOAD CONSTANT D MATRIX 

c ar 

Cc) Da=o Tae For 

C= i I+1 I+1 


9 nn nr reer na 


c = C12 + #43 
Crm rr re 
IF (I.EQ.LINKS) GO TO 100 
CALL MAT44T (ERTIA,I,TD2,I, C12, I) 
CALL MAT44 (A, I+1,DD, I+1, C13, 1) 
CALL ADD44 (C12,I, (13,1, DD, I) 
GO TO 110 
100 CONTINUE 
CALL MAT44T (ERTIA,I,TD2, I, DD, I) 
110 CONTINUE 
(----------—-------------------—----— LOAD CONSTANT C MATRIX 
c I 
C2 :2G M R +A C 
c OU I I It1 I+1 
Cn re eee eo oe a ee eee 
c = cl4 + C15 
Gea 
IF (I.EQ.LINKS) GO TO 120 
CALL MATC1 (AMASS, I,RBAR, I, C14, I) 
CALL MAT41 (A,I+1, CC, I+1, C15, 1) 
CALL ADD11 (C14,1I,C15,I,CC,1I) 
GO TO 130 
120 CONTINUE 
CALL MATC1 (AMASS, I,RBAR, I, CC, I) 
130 CONTINUE 
140 CONTINUE 





Se a eS ee ee oe ee eS ee Ge re See 








aes 5 SR eS ee ee et eS eS eS ee Be 





"ae aoe LOAD PARTIAL T WRT Q 
C “-Eat PA J 

C. Saale Sad ak 

C PQ J=1- oP Q J 

C J J 

(mn ee ee eee 


DO 190 J=1,LINKS 
IF (J.EQ.1) GO TO 150 
CALL MAT44 (TT,J-1,PA,J,PTQ,J) 
GO TO 180 
150 CONTINUE 
DO 170 II=1,4 
DO 160 JJ=1,4 
PTQ(II,JJ,J)=PACII,JJI,J) 
160 CONTINUE 
170 CONTINUE 
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180 CONTINUE 

190 CONTINUE 

C---- ~-——-~~---—--—----------——---- CALC FORCE/ TORQUE 
c | PT T PT 

Cc F = TRI --I DI/I- 6G --I c 

c I | PQ I | PQ I 

c | I | I 


C(- nnsnnr n 


C = C17 = C19 





DO 200 I=1,LINKS 
CALL FIRST (PTQ,1I,DD, C17) 
CALL SECOND (GRAV, PTQ,1I, CC, (C19) 
TAU( I) =C17-C19 
TAU( I) =TAU(I) /10000.0 
200 CONTINUE 
RETURN 
END 
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CF SSSHSSSSHSSKSTSSSKSSSKS SSS SSKSKSSSKESSSSKSHSKSKSSKSSKSESKKKKKKKKKEKKEKEKEKESARNI 
CESSSSKSSSSESSSSKSSSSSSSSSSSSSHKSSSSSSSKSESKSSKSSKEKEKASKSKKSEKEKAKKKEKSEKSESARNMI 
CESSSSSHSSSSSSSSSSSSSHSSSSSSSKSSKSSSSSKSEKSEKSKSEKSTKKSSKKSEKKKKKEKKEKESEKARHID 


SUBROUTINE ARM2 (T,TDD, TAU) 


COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 


/ROB1/ 
/ROB2/ 
/ROB3 / 
/ROB4/ 
/ROBS/ 
/ROB6 / 
/ ROB7/ 
/ROB8/ 
/ROB9/ 


G,PI,D2R,R2D 
ERTIA(4,4,15) , RBAR(4,15) 
SMALLA(15) , SMALLD(15) , ALPHA(15) 
B2XX(15) ,B2YY(15) ,B2ZZ(15) 

AMASS (15) , ACTIA(15) , LINKS 
XBAR(15) , YBAR(15) ,ZBAR(15) 
GRAV(4,15) , LNKTYP(15) 
A(4,4,15),PA(4,4,15) ,PPA(4, 4,15) 
ZERO(15),TT(4,4,15), PTQ(4, 4,15) 


REAL T(15),TDD(15) ,TAU(15) ,DD(4,4,15) ,C4(4,4,15) ,C10(4,4,15),C11(4 
1,4,15) ,C12(4,4,15) ,C13(4,4,15) ,TD2(4,4,15), C17 
INTEGER I,J 
Ca a a INITIALIZE CONSTANT MAdionees 
DO 30 I=1,4 
DO 20 J=1,4 
DO 10 K=1,LINKS 
DD(I,J,K)=0.0 
TD2(I,J,K)=0.0 
C4(I,J,K)=0.0 
C10(I,J,K)=0.0 
C11(I,J3,K)=0.0 
C12(1I,J,K)=0.0 
C13(I,J,K)=0.0 
10 CONTINUE 
20 CONTINUE 
30 CONTINUE 

















C oe 
c See eo LOAD STRIPPED T MATRIX 
c 
C ae oe PA oe 
C T = T ee =-7) 
cS I= J=1 3220 J 
c J 
Cc manenaseneenes ee ree ee me ane ane ee eee ene eee ame aoe 
c = C4 + C11 
(Ce ee rr er ee re oP oe ane ee as ow ae an a a er en een enero enema 
DO 60 J=1,LINKS 
IF (J.EQ.1) GO TO 40 
CALL MAT44 (TD2,J-1,A,J,C4,J) 
CALL MAT44 (TT,J-1,PA,J,(C10,J) 
CALL MATC4 (TDD(J),C10,J,C11,J) 
CALL ADD44 (C4,J,C11,J,TD2,J) 
GO TO 50 
40 CONTINUE 


CALL MATC4 
50 CONTINUE 


(TDD(J),PA,J,TD2,J) 
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60 CONTINUE 


(-—-------—---—---------------—— LOAD CONSTANT D MATRIX 
c aeE 

feepe=J T + A OD 

cece I I I+1 I+1 

Cm nn ree 
c = ¢12 + C13 





DO 90 II=1,LINKS 
t=7-II 
IF (I.EQ.LINKS) GO TO 70 
CALL MAT44T (ERTIA,1,TD2,1, (12,1) 
CALL MAT44 (A,1+1,DD,1+1, (C13, 1) 
CALL ADD44 (C12,1,C13,1,DD, 1) 
GO TO 80 
70 CONTINUE 
CALL MAT44T (ERTIA,1I,TD2,1,DD, I) 
80 CONTINUE 
90 CONTINUE 





(---------- ——-- ---— —-—--— CALC FORCE/TORQUE 
Cc | PT | 

Cc F = TR! -—-I Dp | 

Cc I | PQ I | 

Cc | I | 

Cn a rama 
Cc = C17 

Cc En ee 


DO 100 I=1,LINKS 

CALL FIRST (PTQ,1I,DD, C17) 

TAU( I) =C17 

TAU( I) =TAU(I) /10000.0 
100 CONTINUE 
ee 

RETURN 

END 
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CERES ERRERER ERASE EAE ESA SAAS aS HARRE AEE HEEAEEEE EE EDREEEDEDEEEEROUTPUT 
CHRASAKSRAKRAAAKAARAAKARARKARAKASERSRERASRAKER AKANE KARE YSEKEREEASAOI TPIT 
CRERASEAAERERERASAERERAKSSSRRERASKKAKARSERKSEREARESERESEEEEEEEEESEEEEEEOUTPUT 


SUBROUTINE OUTPUT (J,T, TD, TDD, THETA, THETD, THDT2, TIME, PAYLD) 


COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 


/ROB1/ 
/ROB2/ 
/ROB3 / 
/ROB4/ 
/ROBS/ 
/ROB6/ 
/ROB7 / 


G, PI,D2R,R2D 
ERTIA(4,4,15),RBAR(4,15) 

SMALLA(15) ,SMALLD(15) , ALPHA(15) 

B2XX(15) ,B2YY(15) ,B2ZZ(15) 

AMASS(15) ,ACTIA(15) , LINKS 
XBAR(15) , YBAR(15) , ZBAR(15) 

GRAV( 4,15) , LNKTYP(15) 

/ROB8/ A(4,4,15),PA(4,4,15), PPA(4,4,15) 

/ROB9/ ZERO(15),TT(4,4,15),PTQ(4,4,15) 

/ROB10/ POS(15,100),VEL(15,100),ACC(15,100) 
/ROB11/ DELTA(15,100) , DELTV(15,100) , DELTP(15,100) 
/ROB12/ SIMERR(15) , TOTACC(15), ACCSIM(15,100) , ISTEP, KNOTS 
/ROB13/ T2(15,100) , TLAST(15) , TDLAST(15) , TDDLST(15) 
/ROB14/ POSSIM(15,100),ISIM 


REAL T(15),TD(15),TDD(15) , THETA( 15,100) , THETD(15,100) , THDT2(15,100 
1) , TIME(100) , PAYLD 
INTEGER I,J,KNOTS, ISIM 





WRITE (27,40) J,PAYLD,TIME(J) 


——----—-—— ———--—-——- --——- ————— ACCELERATION 


WRITE (27,50) 

DO 10 K=1,LINKS 

DELTA(K, J) =ABS (THDT2(K,J)-TDD(XK) ) 

WRITE (27,80) K,THDT2(K,J),TDD(K),DELTA(K,J) 
CONTINUE 


(---—--------------------- --—--_-------—---—-- VELOCITY 


WRITE (27,60) 

DO 20 K=-1,LINKS . 

DELTV(K,J) =ABS (THETD(K,J)-TD(XK) ) ; 
WRITE (27,80) K,THETD(K,J),TD(K) ,DELTV(K,J) 

CONTINUE 


(eee rr en ee ee ee 





POS ITION 


WRITE (27,70) 

DO 30 K=1,LINKS 

DELTP(K,J)=ABS (THETA(K,J)-T(K)) 

WRITE (27,80) K,THETA(K,J),T(K),DELTP(K,J) 
CONTINUE 

IF (ISIM.EQ.1) GO TO 40 


———— — -—- -- -—-————-——-—-0UTPUT TO GRAPHICS DATA FILES 
WRITE (41, 80) 
WRITE (42, 80) 
WRITE (43, 80) 
WRITE (44, 80) 
WRITE (45, 80) 
WRITE (46, 80) 


TIME(J) , THDT2(1,J) , TDD(1) 
TIME(J) , THDT2(2,J) , TDD(2) 
TIME(J) , THDT2(3,J) , TDD(3) 
TIME(J) , THDT2(4,J) , TDD(4) 
TIME(J) , THDT2(5,J) , TDD(5) 
TIME(J) , THDT2(6,J), TDD(6) 
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C a ae a nea ee ee Om eee eee oe ee oe -_— 
WRITE (31,80) TIME(J),THETD(1,J),TD(1) 
WRITE (32,80) TIME(J) , THETD(2,J) ,TD(2) 
WRITE (33,80) TIME(J), THETD(3,J),TD(3) 
WRITE (34,80) TIME(J) , THETD(4,J) ,TD(4) 
WRITE (35,80) TIME(J) , THETD(5,J),TD(5) 
WRITE (36,80) TIME(J) , THETD(6,J) ,TD(6) 
C--— ee ee er ene ee en eae 
WRITE (21,80) TIME(J) , THETA(1,J) ,T(1) 
WRITE (22,80) TIME(J) , THETA(2,J) ,T(2) 
WRITE (23,80) TIME(J) , THETA(3,J) ,T(3) 
WRITE (24,80) TIME(J) , THETA(4,J) ,T(4) 
WRITE (25,80) TIME(J) , THETA(S,J),T(5) 
WRITE (26,80) TIME(J) ,THETA(6,J) ,T(6) 
GO TO 70 
mn nn nr tt rn er eee ee ee ee eee eee 
40 CONTINUE 
IF (J.NE.KNOTS) GO TO 70 
WRITE (27,90) ISTEP, KNOTS 
DO 50 II=1,LINKS 
S IMERR( II) =S IMERR( II) / TOTACC( ITI) 
WRITE (27,100) II,SIMERR(II) 
50 CONTINUE 
WRITE (21,110) TIME(J),THETA(1,J), POSSIM(1,J) 
WRITE (22,110) TIME(J) , THETA(2,J),POSSIM(2,J) 
WRITE (23,110) TIME(J) , THETA(3,J),POSSIM(3,J) 
WRITE (24,110) TIME(J),THETA(4,J) ,POSSIM(4,J) 
WRITE (25,110) TIME(J) , THETA(S5,J) , POSSIM(5,J) 
WRITE (26,110) TIME(J) , THETA(6,J) ,POSSIM(6,J) 
70 CONTINUE 





40 FORMAT (/13HKNOT NUMBER =,15,10H PAYLOAD =,F15.7,8H TIME = ,F13.5 


80 FORMAT (3F15.7) 
90 FORMAT (/17HNUMBER OF STEPS =,15,18H KNOTS PROCESSED =,I5) 
100 FORMAT (/13HLINK NUMBER =,14,10H ERROR = ,E18. 8) 
110 FORMAT (3F15.7) 
END 
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CFSSSSSECSSSTSSSKSSSKSKSKKKKKEKEKKKEKE SSSEKEKEEES TEESE EEEEEEEEEESES CONST 
C#SSSSSSSSSSSSSSSSSKKKKKKSKSESKS ESSE KKEKKKKK SEEKS ESE EEEEKESESEEEESSCONST 
CFFFSSESSSESSS SSSA SESSSSSSKESKESE KSEE SS SESKKEEKKE KEL ELEEEKESESEESEES CONST 


SUBROUTINE CONST 
COMMON /ROB1/ G,PI,D2R,R2D 
COMMON /ROB2/ ERTIA(4,4,15) ,RBAR( 4,15) 
COMMON /ROB3/ SMALLA(15),SMALLD(15) , ALPHA(15) 
COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 
COMMON /ROB5/ AMASS(15) ,ACTIA(15) ,LINKS 
COMMON /ROB6/ XBAR(15) , YBAR(15) , ZBAR(15) 
COMMON /ROB7/ GRAV(4,15) , LNKTYP( 15) 
COMMON /ROB8/ A(4,4,15),PA(4,4,15), PPA(4,4,15) 
COMMON /ROB9/ ZERO(15),TT(4,4,15), PTQ(4, 4,15) 
C#XSSSEEKEEEKELEEESEEEESEEESEEEESEESE] OAD UNIVERSAL CONSTANT 
PI=ARSIN(1.0) *2.0 
D2R=PI/180.0 
R2D=180.0/ PI 
G=980.621 
C#FFKSSEKEKEKEKEKEKERERELE SESE EEE] OAD ARM CHARACTERISTICS 
READ (15, *) LINKS, (LNKTYP(I) ,SMALLA( I) , SMALLD( I) , ALPHA( I) ,B2XX(TI), 
1B2YY(I) ,B2ZZ(1I) , XBAR(I) , YBAR(I) ,ZBAR( I) , AMASS(I) , ACTIA(I) , I=1, LINK 
2S) 
C FSEEEKEEE CEES KEES EEESEEEEEEESESS] OAD LINK INERTIA’S 
DO 10 I=1,LINKS 
ERTIA(1,1, I) =AMASS(I) *( (B2YY( I) +B2ZZ (I) -B2XX(I) )/2.0) 
ERTIA(2,2, 1) =AMASS( I) *( (B2XX( I) +B2ZZ( I) -B2YY(I) )/2.0) 
ERTIA(3,3, 1) =AMASS(I) *( (B2XX( I) +B2YY( I) -B2ZZ(I) )/2.0) 
ERTIA(1,4, I) =AMASS(I) *XBAR( I) 
ERTIA(2,4, I) =AMASS( I) *YBAR(TI) 
ERTIA(3,4, I) =AMASS(I) *ZBAR( I) 
ERTIA( 4,1, I) =AMASS( I) *XBAR( TI) 
ERTIA( 4,4, I) =AMASS(I) *1.0 
ERTIA( 4,1, 1)=AMASS(I) *XBAR(I) 
ERTIA( 4,2, 1) =AMASS(I) *YBAR( I) 
ERTIA( 4,3, 1) =AMASS(I) *ZBAR( I) 
ERTIA(1,2,1)=0.0 
ERTIA(1,3,1)=0.0 
ERTIA(2,1,1)=0.0 
ERTIA(2,3,1)=0.0 
ERTIA(3,1,1)=0.0 
ERTIA(3,2,1)=0.0 
10 CONTINUE 
C SSSEEEEEEEKEEKEEEEKEEEEEEEKEEKESKE TNO Q MATR IX B LINK MASS CENTERS 
DO 30 I=1,4 
DO 20 J=1,LINKS 
RBAR(I,J)=0.0 
20 CONTINUE 
30 CONTINUE 
RBAR(3,1)=ZBAR(1) 
RBAR(4,1)=1.0 
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RBAR(1, 2) =XBAR(2) 
RBAR(3 ,2) =ZBAR(2) 
RBAR(4,2)=1.0 
RBAR(3,3) =ZBAR(3) 
RBAR(4,3)=1.0 
RBAR(2, 4) =YBAR(4) 
RBAR (4, 4)=1.0 
RBAR(3,5)=ZBAR(5) 
RBAR(4,5)=1.0 
RBAR (3 , 6) =ZBAR(6) 
RBAR(4,6)=1.0 

C#SFSSSSSSesseSeeeeesseseSee eee eee7 ERGO AND ALPHA~->RAD MATRICES 
DO 39 IJ=1,15 
ZERO(I)=0.0 

39 CONTINUE 
DO 40 I=1,LINKS 
ALPHA( I) =ALPHA( I) *D2R 

40 CONTINUE 

CFSS SSH eSeHeeee ee eee ees see eee TNT TITALIZE A MATRICES 
DO 80 J=1,4 
DO 70 K=1,4 
IF (J.EQ.K) GO TO 50 
A(J,K,1)=0.0 
TI(J,K,1)=0.0 
GO TO 60 

50 CONTINUE 
A(J,J,1)=1.0 
TI(J ,J,1)=1.0 

60 CONTINUE 

70 CONTINUE 

80 CONT INUE 
RETURN 
END 
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C FHSS SSeS See esses EEEEEEEEESEEEEEEEEEEEESESE EK TNEMT 
Cc SHKSASSKSSSKSSSSKASSSSKKSKKEKSKKAKEKANKKKEKEEEEKEEESEEEEEEEEESESESEKTNEMT 
C SHSSTSKKKKSSSSKSSSKSSKSSASKAKSL ASSET KEKKEKSE SHEKELS KSE SESE EEE EEK TINEMT 


10 
C 


20 
30 








SUBROUTINE KINEMT (T, PAYLD) 

COMMON /ROB1/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15),RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) , ALPHA(15) 
COMMON /ROB4/ B2XX(15) ,B2YY(15),B2ZZ(15) 
COMMON /ROBS/ AMASS(15),ACTIA(15) , LINKS 
COMMON /ROB6/ XBAR(15), YBAR(15) , ZBAR(15) 
COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROB8/ A(4,4,15),PA(4,4,15),PPA(4,4,15) 
COMMON /ROB9/ ZERO(15),TT(4,4,15) , PTQ(4, 4,15) 
REAL T(15),PAYLD 

coer eren eee eren neers ner meen awn wer oeereceneme—— COMPUTE INDIVIDUAL LINK A=MATRiRerS 
DO 10 I=1,LINKS 

A(1,1,1I)=COS(T(I) ) 

A(1,2, I) =—-COS(ALPHA(I) ) *SIN(T(I) ) 
A(1,3,1)=SIN(ALPHA(I) ) *SIN(T(I) ) 

A(1,4, I)=SMALLA(I) *COS(T(I) ) 

A(2,1, I)=SIN(T(I) ) 

A(2,2,1I)=COS( ALPHA(I) ) *COS(T(I) ) 

A(2,3, 1)=-SIN(ALPHA(I) ) *COS(T(TI) ) 
A(2,4,1I)=SMALLA(I) *SIN(T(TI) ) 

A(3,1, I)=0.0 

A(3,2,1I)=SIN(ALPHA(T) ) 

A(3,3,1I)=COS(ALPHA(I) ) 

A(3,4, I) =SMALLD(I) 

A(4,1,1)=0.0 

A(4,2,1)=0.0 

A(4,3,1)=0.0 

A(4,4,1I)=1.0 

CONTINUE 

ee a nnn een wncacac———— COMPUTE AND LOAD 1ST PARTIAL A-=MATa 
DO 30 I=1,LINKS 

PA(1,1,1I)=-SIN(T(TI) ) 

PA(1,2, I)=-COS(T(I) ) *COS( ALPHA(I) ) 
PA(1,3,1)=COS(T(I) ) *SIN( ALPHA(T) ) 
PA(1,4,1)=-SMALLA(I) *SIN(T(I) ) 
PA(2,1,1)=COS(T(TI) ) 

PA(2,2, I)=-SIN(T(I) ) *COS( ALPHA(I) ) 
PA(2,3,1)=SIN(T(I) ) *SIN(ALPHA(T) ) 

PA(2,4, 1I)=SMALLA( I) *COS(T(T) ) 

DO 20 K=1,4 

PA(3,K, 1I)=0.0 

PA(4,K,1I)=0.0 

CONTINUE 

CONTINUE 
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wre nnn ner arenavermnmvaven—m= COMPUTE AND LOAD 2ND PARTIAL A-MATRIX 
DO 50 I=1,LINKS 
PPA(1,1,1)=-COS(T(I) ) 
PPA(1,2,1)=SIN(T(I) ) *COS( ALPHA( I) ) 
PPA(1,3,1) =-SIN(T(I) ) *SIN( ALPHA(I) ) 
PPA(1,4, I) =-SMALLA(I) *COS(T(I) ) 
PPA(2,1,1)=-SIN(T(I) ) 
PPA( 2,2, 1)=-COS(T(I) ) *COS( ALPHA(I) ) 
PPA(2,3,1) =COS(T(I) ) *SIN(ALPHA(I) ) 
PPA(2,4, I) =-SMALLA(I) *SIN(T(I) ) 
DO 40 K=1,4 
PPA(3,K, 1) 
PPA(4,K, I) 
40 CONTINUE 
50 CONTINUE 
eee (AT) LINK TRANSPOSITION T-MATRICES 
DO 110 I=1,LINKS 
C----- oo ——— 
DO 100 J=1,4 
DO 90 K=1,4 
TI(J,K, I)=0.0 
DO 80 L=1,4 
IF (I.EQ.1) GO TO 60 
TT(J,K, 1) =TI(J,K,1)+TI(J,L, 1-1) *A(L, K, I) 
GO TO 70 
60 CONTINUE 
TT(J,K,1)=A(J,K,1) 
70 CONTINUE 
80 CONTINUE 
90 CONTINUE 
100 CONTINUE 


C---------------—- 


110 CONTINUE 
Cer rrr rrr rn 
DO 120 I=1,LINKS 
GRAV(1,1)=0.0 
GRAV(2,1I)=0.0 
GRAV(3,1)=G 
GRAV(4,1I)=0.0 
120 CONTINUE 
RETURN 
END 


=0.0 
=0.0 





LOAD GRAVITY MATRIX 
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CeSSSSSSHSSSSSSSSKSSKSSKSKEKKSSSSKSESSSSKAKAKAAK SESSA SESH SKKSKEKKKSEKESE SESE SETRAT 
C#FSFPSSSSKKSTSESSTSTKKKSEKKHKSKSKSKSKSKSKSKSASKATELSSKHKSKKSSKSESKSELSHKSEREKEKSHKKEKTRAT 
CFSSSSSHSSKSKKKSKSKSKSTSATESSSSSSKSEKSSKR STATA TLL KKKKKKKKHKKSEKKHKKE KEK TRAT 


10 
20 


SUBROUTINE TRAJ (KNOTS, TIME, THETA, THETD, THDT2, TU) 
COMMON /ROBS/ AMASS(15),ACTIA(15) ,LINKS 
COMMON /ROB13/ T2(15,100) , TLAST(15) , TDLAST(15) , TDDLST(15) 


COMMON /ROB14/ POSSIM(15,100),ISIM 
REAL THETA(15,100) , THETD(15,100) , THDT2(15,100) , TU(15,100) , TIME(100 


1) 


INTEGER I,J,KNOTS 
READ (20, *) KNOTS 
DO 20 I=1,LINKS 
DO 10 J=1, KNOTS 
READ (20,*) TIME(J),THETA(I,J),THEID(1I,J), THDT2(1,J) 
T2(1I,J)=THDT2(1I,J) 
CONTINUE 
CONTINUE 
DO 30 J=1, KNOTS 
READ (50,*) TDUM 
READ (50,*) TU(1,J),TU(2,J) ,TU(3,J),TU(4,3) ,TU(5,3),TU(6,S) 
WRITE (27,40) TDUM,TU(1,J),TU(2,J3),TU(3,J),TU(4,J),TU(5,J) ,TU(6,J) 
CONTINUE 
KNOTS=10 
RETURN 
FORMAT (7F10.4) 
END 
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CF SSESEKETAERES SKE EARERE EERE RARER ERESEHAEHERERARHESEETARAREKSERSE SSA ESAKAESERRORS 
C#ESEEEESSEAE KAKA EEEEAEKEKTAKAEKAKEKE REET STHKKEAKESHEAKRSEKSEAKAEKKAKESEARERAELPRORS 
CFESA KAEAE SAKES REEKE RAK AE KEKE KAETHSASEKAKREERAEKEKSEAKAEKESSESKAERRORS 


SUBROUTINE ERRORS (KNOTS, THETA, THETD, THDT2) 
COMMON /ROB5/ AMASS(15),ACTIA(15) , LINKS 
COMMON /ROB11/ DELTA(15,100) , DELTV(15,100) , DELTP(15,100) 
COMMON /ROB14/ POSSIM(15,100), ISIM 
REAL THETA(15,100) , THETD(15,100) , THDT2(15,100) , DELA, DELV, DELP, DN,A 
1A, AV, AP, MINA, MINV, MINP, MAXA, MAXV, MAXP, TDDSUM, TDSUM, TSUM 
(-------——- INITIALIZE VARIABLES 
DO 20 K=1,LINKS 
DELA=0 .0 
DELV=0.0 
DELP=0 .0 
TDDSUM=0.0 
TDSUM=0.0 
TSUM=0.0 
MAXA=0 .0 
MAXV=0.0 
MAXP=0.0 
MINA=100.0 
MINV=100.0 
MINP=100.0 
C--—------—---------—- -——-- -- -- - -—-———-- FIND MAX/ MIN 
DO 10 J=1, KNOTS 
IF (ABS( DELTA(K,J)).GT.MAXA) MAXA=ABS(DELTA(K,J) ) 
IF (ABS(DELTV(K,J)).GT.MAXV) MAXV=ABS(DELTV(K,J) ) 
IF (ABS(DELTP(K,J)).GT.MAXP) MAXP=ABS(DELTP(K,J) ) 
IF (ABS(DELTA(K,J)).LT.MINA) MINA=ABS(DELTA(K,J) ) 
IF (ABS(DELTV(K,J)).LT.MINV) MINV=ABS(DELTV(K,J) ) 
IF (ABS(DELTP(K,J)).LT.MINP) MINP=ABS(DELTP(K,J) ) 
C--~---—----- ------ ---~---- --~--—---~FIND DIFFERENCE/ ERRORS 
DELA=DELA+ABS(DELTA(K,J) ) 
DELV=DELV+ABS(DELTV(K,J) ) 
DELP=DELP+ABS(DELTP(K,J) ) 
TDDSUM=TDDSUM+ABS ( THDT2(K,J) ) 
TDSUM=TDSUM+ABS ( THETD(K,J) ) 
TSUM=TSUM+ABS ( THETA(K,J) ) 
10 CONTINUE 
AA=DELA/ TDDSUM 
AV=DELV/TDSUM 
AP=DELP/TSUM 
WRITE (37,30) AA,MINA, MAXA 
WRITE (37,30) AV,MINV, MAXV 
WRITE (37,30) AP,MINP, MAXP 
20 CONTINUE 


(me we wes er ee 
RETURN 

30 FORMAT (3F18.7) 
END 
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SSSEHSSSESSSSESSSSSSSSSESSSSSSSESSSSESSSSKSLSEKSESSEKSESE SEE ESTES SE SSELSESERORS IM 
SSSSHESSESHESSSSSSSSKE SESS SSSSKSESTSESSSESSES SELES SE SSESELESELESESELESESESLOBS TM 
SSEKECSESSSSSESSHESHESSESSESSESSSSESSSSESSE SS SSE SESE KEES SESE SESE SESE SLESESEP OBS TM 


SUBROUTINE ROBSIM (KN,T,TD,TDD, TAU, FMK, PAYLD, TIME) 


COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 
COMMON 


/ROB1/ 
/ROB2/ 
/ROB3 / 
/ROB4/ 
/ROBS / 
/ROB6/ 
/ROB7/ 
/ROB8/ 
/ROB9/ 


G, PI,D2R,R2D 
ERTIA(4,4,15) , RBAR(4,15) 
SMALLA(15) , SMALLD(15) , ALPHA(15) 
B2XX(15) , B2YY(15) ,B2ZZ(15) 
AMASS(15) , ACTIA(15) , LINKS 
XBAR(15) , YBAR(15) , ZBAR(15) 
GRAV(4,15) , LNKTYP(15) 
A(4,4,15),PA(4,4,15) , PPA(4,4,15) 
ZERO(15) ,TT(4,4,15) , PTQ(4,4,15) 


/ROB10/ POS(15,100) , VEL(15,100) ,ACC(15,100) 

/ROB12/ SIMERR(15) ,TOTACC(15) , ACCSIM(15,100) , ISTEP, KNOTS 
/ROB13/ T2(15,100),TLAST(15) , TDLAST(15) , TDDLST(15) 
/ROB14/ POSSIM(15,100) , VELSIM(15,100) ,ACLSIM(15,100) , ISIM 


REAL T(15) ,TD(15) , TDD(15) , TAU(15) , FMK(15) , HJ (15) ,B(15) , EJ (15) ,H(15 


1,15) ,WORK(45,3) ,TIME(100) , TDOT2(100) , TDOT(100) , TPOS(100) , TDDSIM(15 
2),TDSIM(15) ,TSIM(15) , TDOLD(15) , DELTT, STEPSZ, PAYLD 
INTEGER I,J,KN,LN 
IF (KN.EQ.KNOTS) GO TO 120 
IF (ISIM.NE.1) GO TO 120 
a 











DO 30 LN=1,LINKS 

IF (KN.NE.1) GO TO 10 
TDDS IM(LN) =TDD(LN) *D2R 
TDS IM(LN) =TD(LN) *D2R 
TS IM(LN)=T(LN) *D2R 

GO TO 20 
CONTINUE 
TDDS IM(LN) =TDDLST(LN) 
TDS IM(LN) =TDLAST (LN) 
TS IM(LN) =TLAST(LN) 
CONTINUE 
CONTINUE 
DELTT=TIME( KN+1) —-TIME( KN) 

STEPSZ=DELTT/ FLOAT( ISTEP) 

DO 100 K=1, ISTEP 

TDOLD(LN) =TDS IM(LN) 

DO 40 LN=1,LINKS 

TDS IM(LN) =TDDS IM(LN) *STEPSZ+TDS IM(LN) 

TS IM(LN) =0.5 *STEPSZ*(TDSIM(LN) +TDOLD(LN)) 
CONTINUE 


~——--~-~---SQLVE FINITE DIFFERENCE STEPS 


CALL KINEMT (TSIM, PAYLD) 
DO 50 I=1,15 


B(I)=0. 


0 


CONTINUE 
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C----—- 


60 


C----— 


90 


CALL ARM1 (TSIM, TDSIM, ZERO, FMK,B) 


DO 80 I=1,LINKS 

DO 60 J=1,LINKS 

HJ (J) =0.0 

EJ (J) =0.0 

CONTINUE 

EJ (I)=1,0 

CALL ARM2 (TSIM,EJ,HJ) 
DO 70 J=1,LINKS 
H(J,1I)=HI (J) 

CONTINUE 

CONTINUE 

DO 90 I=1,LINKS 

TDDS IM( I) =TAU( I)-B( I) 
CONTINUE 

IER=0 


<<a... ..):ClUU CM COMPUTE LINK ACCELERATIONS 


100 
C- 


110 


C(--- nr 


120 


CALL TDDSOL (H,15,LINKS,LINKS,TDDSIM,15,1,0, WORK, IER) 
CONTINUE 
nen a — COMPUTE LINK ERROR DIFFERENCES 
DO 110 LN=1,LINKS 
TDDLST(LN) =TDDS IM(LN) 
TDLAST (LN) =TDS IM(LN) 
TLAST(LN) =TS IM(LN) 
POSS IM(LN, KN) =TSIM(LN) *R2D 
VELS IM(LN, KN) =TDSIM(LN) *R2D 
ACLS IM(LN, KN) =TDDS IM(LN) *R2D 
ACCS IM(LN, KN) =TDDS IM(LN) *R2D 
S IMERR (LN) =S IMERR (LN) +ABS (T2 (LN, KN) ~ACCS IM(LN, KN) ) 
TOTACC(LN) =TOTACC(LN) +ABS(T2 (LN, KN) ) 
CONTINUE 





CONTINUE 
RETURN 
END 
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C#SSSSSSSSSSKSSSSKSSKSKSEKSEKKEKKLEKERLESLESHESERHESKEESEEESEESSEESEESAVET SOL 
C#FSSSSSSESCSSSKSSSSSESSSSS SSS SSSSSEKEKSSSESESSKKEKKEKKSEKEEKEKEKKLEKELKSVET SOL 
CFSSSSSSSSSESSSSSSSSSSKLTSSSSSSSSSSSSSSSSESKSKSEKSEKEKEKKLKEKSKESEKEKSASEVET SOL 


SUBROUTINE VELSOL(X, Y,Z,NDIM) 
REAL X(100) , ¥(100) ,Z(100) 


C3 orn ns nr rn 
SUM2=0. 
IF(NDIM-1)4,3,1 
(-—---~-~-~---------------------------~—--- INTEGRATION LOOP 
1 DO 2 I=2,NDIM : 
SUM1=SUM2 


SUM2=SUM2+.5*(X(1I)-X(I-1) ) *(Y( 1) +Y(I-1) ) 
2 Z(I-1)=SUM1 
3 Z(NDIM) =SUM2 
4 RETURN 

END 


CFSSSSSSSSSHSSSSSSLSKSKSESKKKKEKEKEKEKEKEKEKEKEKEKESEEESEREKEKESSESEPOSSOL 
C#SSESSSSSSSSESSEKLSSSSSSSSSESSESSESSKSEKSEKEKKEKEKELSESLEKERSERSESSES“SEPOSSOL 
C#FSSSSSSESSSSSSESSSSSSSSSSSSKSEKSEKKTKEKEKKKEKLEKESE SELES ESESEKESESESSESPOSSOL 


SUBROUTINE POSSOL(X, Y, DERY,Z,NDIM) 
REAL X(100), Y(100) , DERY(100) ,Z(100) 
Ce nr rr rr eee 
SUM2=0. 
IF(NDIM—-1)4,3,1 
ana amnmmmians I (k| LG = 
1 DO 2 [=2,NDIM 
SUM1=SUM2 
SUM2=.5*(X(I)-X(I-1)) 
SUM2=SUM1+SUM2 *( (Y( I) +¥( 1-1) )+.3333333 *SUM2 *( DERY( I-1) -DERY(I) ) ) 
Z(I-1)=SUM1 
Z (ND IM) =SUM2 
RETURN 
END 


> W dM 
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C SHES ESASEAHEAEAHSE SEES KA KEES EEE KEKE ARESARAKEREKEEKSEKEEAEKHESKASETRANSP 
C FESS SKSAKSESESEKREKSE SHEESH SEEKER EKEKEEEEEEESEEKETRANSP 
C SHEESH KEKSSEESKEE SEES EKEEHKESHEEKEEKEEKEETRANSP 


REAL A(4,4,15),TT(4,4,15,2) ,T(15) , TE(15,100,2) , SMALLA(15) , SMALLD(1 
15), ALPHA(15) , TDUM 
INTEGER KNOTS 
(------------------—----—-------——- COMPUTE INDIVIDUAL LINK A-MATRICES 
SERR1=0.0 
SERR2=0.0 
SERR3=0.0 
PI=ARSIN(1.0) *2.0 
R2D=180.0/PI 
D2R=PI/180.0 
LINKS=6 
DO 21 I=1,LINKS 
SMALLA( I) =0.0 
SMALLD( I) =0.0 
ALPHA(I)=0.0 
21 CONTINUE 
SMALLA(2) =43.2 
SMALLA(3)=1.9 
SMALLD(3)=12.5 
SMALLD( 4) =43.2 
ALPHA(1)=~90.0*D2R 
ALPHA(3)=90.0*D2R 
ALPHA( 4) =—-90.0*D2R 
ALPHA( 5) =90.0*D2R 
(Coe tee eee ee ee eee 
I=0 
7 I=I+1 
READ( 21, *, END=1) TDUM, TE(1, 1,1), TE(1, I, 2) 
1 CONTINUE 
READ( 22, *, END=2) TDUM, TE(2, 1,1) , TE(2, I, 2) 
2 CONTINUE 
| READ(21, *, END=3) TDUM, TE(3, 1,1) ,TE(3, 1,2) 
3 CONTINUE 
READ( 21, *, END=4) TDUM, TE(4, 1,1), TE(4, I, 2) 
4 CONTINUE 
READ(21, *, END=5) TDUM, TE(5, 1,1) ,TE(5, 1,2) 
5 CONTINUE 
READ(21, *, END=6) TDUM, TE(6,1I,1) ,TE(6, 1,2) 
GO TO 7 
6 CONTINUE 
KNOTS=I-1 
DO 11 JJ=1,KNOTS 
DO 12 KK=1,2 
DO 10 I=1,LINKS 
T( I) =TE(I,JJ ,KK) *D2R 
A(1,1,1I)=COS(T(I) ) 
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10 


113 
112 
111 


C----—- 


12 





A(1,2,1I)=—-COS( ALPHA(TI) ) *SIN(T(I) ) 
A(1,3,1I)=SIN( ALPHA(I) ) *SIN(T(I) ) 
A(1,4,1I)=SMALLA(I) *COS(T(I) ) 

A(2,2,1)=COS( ALPHA(I) ) *COS(T(T) ) 
A(2,3,1)=-SIN( ALPHA(I) ) *COS(T(T) ) 
A(2,4,1)=SMALLA(CI) *SIN(T(T) ) 

A(3,1,1)=0.0 

A(3,2,1)=SIN( ALPHA(T) ) 

A(3,3,1)=COS( ALPHA(T) ) 

A(3,4, I) =SMALLD(T) 

A(4,1,1I)=0.0 

A(4,2,1)=0.0 

A(4,3,1I)=0.0 

A(4,4,I)=1.0 

CONTINUE 

nn LOAD LINK TRANSPOSITION foia eee 
DO 111 I=1,LINKS 

DO 112 J=1,4 

DO 113 K=1,4 

TT(J,K,1I,KK)=0.0 

CONTINUE 

CONTINUE 

CONTINUE 

DO 110 I=1,LINKS 

DO 100 J=1,4 

DO 90 K=1,4 

TI(J ,K, I, KK) =0.0 

DO 80 L=1,4 

IF (I.EQ.1) GO TO 60 

TT(J,K, I, KK) =TT(J,K, I, KK) +TT(J,L, I-1, KK) *A(L,K, I) 
GO TO 70 

CONTINUE 

TT(J,K,1,KK)=A(J ,K,1) 

CONTINUE 

CONTINUE 

CONTINUE 

CONTINUE 

CONTINUE 

IF (KK.EQ.1)WRITE(61,200)TT(1,4,6,1),T1(2,4,6,1),1T1T(3,4,6,1) 
IF (KK.EQ.2)WRITE(62,200)TT(1,4,6,2),1T1%(2,4,6,2) ,1T1(3,4,6,2) 
CONTINUE 

ERR1=ABS (TT(1,4,6,1)-TT(1,4,6,2))/TT(1,4,6,1) 
ERR2=ABS (TT(2,4,6,1)-TT(2,4,6,2))/T1T(2,4,6,1) 
ERR3=ABS (TT(3,4,6,1)-TT(3,4,6,2))/TT(3,4,6,1) 
WRITE( 63,200) ERR1, ERR2 , ERR3 

SERR1=ERR1+SERR1 

SERR2=ERR2+SERR2 
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11 


200 
230 


SERR3=ERR3 +SERR3 

CONTINUE 

ER1=SERR1/ FLOAT( KNOTS) 

ER2=SERR2/ FLOAT( KNOTS) 
ER3=SERR3 / FLOAT( KNOTS) 
WRITE(63,230) ER1,ER2,ER3 

RETURN 

FORMAT (3F15.8) 

FORMAT (/'TOTAL ERRORS =',3F15.8) 
END : 


CFSSFESSKKAESEKEKSKSKSEKSKSTKSKSEKRSEKSEKRSKAKSEKRSEKCSKSEKSAKA RHEE KKAKEKAEKSAKVWATAA 
C#SSKEKSTSKSEKSKSKSSEKESEKRSESKTSKSTSAKAKSTKAKSKHKSTKA KSAT SAKAKSEKRESAKSERSAKAAVATAA 
C#ESEKSKKEKSSEKLKSEKSEKRKEKSEKRKSEKRKSAKSKSEKSEKSEKRASAKRSAKRHAKSKAKSESKKSEKRAAKSASKARVANATAA 


10 
20 
30 


SUBROUTINE MAT44 (AA,1I1,BB,J1,CC,K1) 
REAL AA(4,4,15),BB(4,4,15),CC(4,4,15) 
INTEGER 11,J1,K1 

DO 30 J=1,4 

DO 20 K=1,4 

CC(J ,K,K1)=0.0 

DO 10 L=1,4 
CC(J,K,K1)=CC(J ,K, K1) +AA(J,L, 11) *BB(L,K,J1) 
CONTINUE 

CONTINUE 

CONTINUE 

RETURN 

END 


C#FSKSKCKSKSESLSESKSESKSKSESKSKSESKKSESKSESKSE KES KRESKSSRSESAKSKKAKSEKHKAEKKSKAKEKASKAVATAAT 
CESSES SKSESKKSESKSESSKSSHKSTKKSESAAAKSTAKRAESAASTKAKREKHKSAKRSLAKSLKAKKSKKEKKEAKENMATAAT 
CFSSSHTKSEKKATSKASTSKSSKAKSTSAASAKSSTSKKSTSKKSESKHKAKKKAKSKKAKKKEKKESKKKAKKASKSEMATAAT 


10 


30 


SUBROUTINE MAT44T (AA,11,BB,J1,CC,K1) 
REAL AA(4,4,15),BB(4,4,15),CC(4,4,15) 
INTEGER 11,J1,K1 

DO 30 J=1,4 

DO 20 K=1,4 

CC(J ,K,K1)=0.0 

DO 10 L=1,4 
cC(J,K,K1)=CC(J,K,K1)+AA(J,L, 11) *BB(K,L,J1) 
CONTINUE 

CONTINUE 

CONTINUE 

RETURN 

END 
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CFS SSSSSSESESESSES SSS SK ESSTSE SSS SESS SSSSES SSS ESSE SE ESSSE SESE SESSESSESKSE MATCH 
CFS SSSSSESSS SE SSSSSSSE SSE SESS SSSSSSSSE SS SSS SSE SSSSES SESS STE SESESESSESSESVWATCA 
CHSSSSSSSSSS SSE SESSSE STS SSE STS SSE SESE SESE SLESE SESE SESE SESESESLESEKESEMATCH 


10 
20 


SUBROUTINE MATC4 (C1, AA,11,BB,J1) 
REAL AA(4,4,15) ,BB(4,4,15) ,C1 
INTEGER 11,J1 

DO 20 I=1,4 

DO 10 J=1,4 
BB(I,J,J1)=C1*AA(I,J, 11) 

CONTINUE 

CONTINUE 

RETURN 

END 


CFSSSSSSSSSSSSSS SS SSSSSSSSSSSSSSSSSSSSSTSESSESSESSSESESSEESKE LESSEE SMATCL 
CFSSSSSESSESSEESSSSESSSSSES SST SSESSSSSSSSSSSSSESSESSESSSESSSESSESSE CELE SKSSEMATCI 
CFSSSSESESSSSSSSESTSSTSSSSSSSSSSSSESSSSESSE SESE SES ESTEE LESSEE ESEESESSEVSEMAT CI 


10 


SUBROUTINE MATC1 (AA,1I1,BB,J1,CC,K1) 
REAL AA(15),BB(4,15) ,CC(4,15) 
INTEGER 11,J1,K1 

DO 10 I=1,4 

CC(I,K1)=AA( 11) *BB(I,J1) 

CONTINUE 

RETURN 

END 


CFSSSESSSSESSE SS SSSTSTESSESSSESSSESESSEESESESESESCESSESEEESE SESS SESESESSEESESESENMATAL 
CFSSSSESSSSESSSSSSESSSSE STS SESS SSE SSE SSS SE SESSESESE SES SESE SESEESESSESESSESESSEMATAL 
CF SSSESSSESSSSSSSSSE SESS SSS SSS SSSSSSSESSSSESSSSE SES SSSSESSESS“SESSESEESEESMATAL 


10 
20 


SUBROUTINE MAT41 (AA,1I1,BB,J1, CC, K1) 
REAL AA(4,4,15) ,BB(4,15) ,CC(4,15) 
INTEGER 11,J1,K1 

DO 20 I=1,4 

CcC(I, K1)=0.0 

DO 10 J=1,4 
CC(1I,K1)=CC(I, K1)+AA( I,J, 11) *BB(J,J1) 
CONTINUE 

CONTINUE 

RETURN 

END 
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CFFSKSKSSHSKSSESSKAKSASESKSE SEEKS KSEKKEKSKKSESSSSTHKKKKKKEKKKSKESKKSEN ATI A 
CESSSSKSSAKSEAEKSEKESSEKSKAKSEKSEKEKKKKKSSSSSSKKKSEKKSEKKSKKKKEKKKESKEKSMATI 4 
C#SSSSKKSKSKSSKKEKEKSKKKKKKSKKESSKSEKKSKSKKKSESSEKSKKKSKKSKSKKKKEKKKKKKKKSENWAT I 4 


10 
20 


SUBROUTINE MAT14 (AA, 1I1,BB,J1, CC, K1) 
REAL AA(4,15),BB(4,4,15),CC(4,15) 
INTEGER 11,J1,K1 

DO 20 I=1,4 

CC(1I,K1)=0.0 

DO 10 J=1,4 
CC(I,K1)=CC(1I,K1)+AA(J,1I1) *BB(J,1I,J1) 
CONTINUE 

CONTINUE 

RETURN 

END 


CeSSSSKSSKKKKESSSSKSKKKSKESKKKEKSKSEKESEKSEKEKEKEKKTKKKKKKKKKEKKKKKANHNDYAA 
CFSSSKEEKEKSESSEKKKKSSSSS SKK SKKSSSKSKKSKTKKKKKKKKKKKKKKKKAKEKKEKKANHNDYAA 
CFSSECSSSKKKKSSSSSSSSSSSSEKSKEKEKSEKKKSSEKSSKKK SKS EK SKKKKEKKKSKSKSANNDYAA 


10 
20 


SUBROUTINE ADD44 (AA,11,BB,J1,(CC,K1) 
REAL AA(4,4,15),BB(4,4,15) ,CC(4,4,15) 
INTEGER 11,J1,K1 

DO 20 I=1,4 

DO 10 J=1,4 
CC(I,J,J1)=AAC(I,J,11)+BB(I,J,K1) 
CONTINUE 

CONTINUE 

RETURN 

END 


CFSESSSKKSSSSKEKKESKSESKSESSSSKKKKKKSEKSEKEKESEEREREAESEEKEKKESSKESKEADDHYI I 
CFSESSKEKSEKSESSKASSSKKSKEKKSKKSKEKKSEKEKEKREKKSKKKKKKEKESKKEKKKEKSESKKANDYHYI I 
C#FSSHEEKSKEKKSEKKSESSEKSKKSESEKSEKAKSSEKSKSEKKKKKKEKEKEKSEKESKESEESKEESSEESADDI I 


10 


SUBROUTINE ADD11 (AA, 1I1,BB,J1, CC, K1) 
REAL AA(4,15),BB(4,15) ,CC(4,15) 
INTEGER I1,J1,K1 

DO 10 I=1,4 

CC(1I,K1)=AA(I, I1)+BB(1I,J1) 

CONTINUE 

RETURN 

END 
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CEEETHSEHRESE EAE EHEEE EES SEEESESEEEEEESEEEEEEEEEEESEEESEEEE SEES ESADDALL 
CECESEREEERESES SEE RETEST ERESEESESEEEEEEEEEEEEEEEE EES EEEEEEEEESESSEADNDALL 
CHFSSESESRSSSE SHAS HES SEA THE SESE ESEEESTESEEEEEEE SEES EEEEE SEE OEEEEEADDALL 


10 


SUBROUTINE ADDALL (AA,11,BB,J1,CC,K1,DD,L1,EE, M1) 

REAL AA(4,4,15) ,BB(4,4,15) ,CC(4,4,15),DD(4,4,15) ,EE(4,4,15) 
INTEGER 11,J1,K1,L1,Ml1 

DO 20 I=1,4 

DO 10 J=1,4 
EE(1I,J,M1)=AA(I,J,11)+BB(1I,J,J1)+CC(1I,J,K1)+DD(I,J,L1) 
CONTINUE 

CONTINUE 

RETURN 

END 


CFF FSSSSSSSSSSSSSSSESSSSSSSSSSSESSESTSESSSTS SESE SETHE HEEEHKLERERNE*ESE*C SAT RST 
CFSSSSSSSSSESSESSSSSSSSSSSSSESSSS SESS KSSH SSKESEESESESESTSESKESRE*NESSES* SAT RST 
CFSSFSSESSSHESSSSSSSSSEKSSSSE SSS SSSSE SHE SESE THES KKE SHEE HEEEEEEESEESSSESELTRST 


10 


SUBROUTINE FIRST (PTQ,11,DD,(C17) 

REAL PTQ(4,4,15),DD(4,4,15) ,CC1(4,4,15),C17 
INTEGER I1 

CALL MAT44 (PTQ,11,DD,11,CC1, 11) 

C17=0.0 

DO 10 I=1,4 

C17=C17+CC1(1,1,11) 

CONTINUE 

RETURN 

END 


CFSFSSSSSSESSSSS SSS SSSSSSSTSSTSTSSSTSSSSESKSKSEKKKKKKEKKETEEKEVSTSE*EKEESSEEGECOND 
CFF SSSSSSSSSSSSSESSSS SESE SCSSKSESSESSSESKESKSSSSSHKCKKLKKHE LTE KLESREKEEEKELESECOND 
CFFSSSSSSSSSSESSESSESSSSESSESSSESESESHHE SKE KKEKESE SE KLEEELELESLEEEKELESL*ESSECOND 


10 


SUBROUTINE SECOND (GRAV, PTQ,11,CC, C19) 

REAL GRAV(4,15) ,PTQ(4,4,15) ,CC(4,15) ,CC2(4,15) , C19 
INTEGER [1 

CALL MAT14 (GRAV,I1,PTQ,1I1,CC2,11) 

C19=0.0 

DO 10 J=1,4 

C19=C19+CC2(J,11) *CC(J, 11) 

CONTINUE 

RETURN 

END 
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